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SUMMARY 


This  report  describes  the  work  performed  by  Battel! e  Columbus 
Division  on  a  project  that  was  part  of  the  DARPA  Adaptive-Suspension 
Vehicle  Program.  The  Battelle  project  consisted  of  two  major  tasks. 

The  first  task  involved  the  guidance  of  the  adaptive-suspension  vehicle; 
the  goal  was  to  develop  a  computer  system  which  would  enable  the  vehicle 
to  traverse  rough  terrain.  The  second  task  concerned  the  design,  fabrication, 
and  testing  of  a  safety  valve  for  the  foot  lift-circuit  of  the  vehicle's 
leg.  Those  two  tasks  are  discussed  separately  in  this  report. 


Guidance  Task 

Work  on  the  guidance  problem  involved  the  development  of  a 
computer  system  to  determine  vehicle  trajectories  and  leg  motions  that 
would  enable  the  vehicle  to  traverse  rough  terrain.  The  system  uses 
the  operator's  requested  vehicle  velocities,  information  about  the  vehicle 
state,  and  data  from  a  terrain-scanning  system  in  the  determination 
of  those  trajectories  and  leg  motions. 

The  hardware  on  which  the  algorithms  described  above  are  implemented 
comprises  six  boards  communicating  over  an  Intel  Multibus.  Three  of 
the  boards,  one  board  containing  special-purpose  circuitry  and  two  Intel 
i SBC  86/30  processor  boards,  receive  data  from  the  the  terrain-scanning 
system,  convert  them  to  elevation  information,  and  then  store  that  information 
on  a  512-kilobyte  memory  borad.  The  other  two  boards  are  two  more  Intel 
iSBC  86/30  processor  boards.  One  of  them  generates  the  vehicle  deceleration 
plans  using  the  terrain  elevation  map  stored  on  the  memory  board,  as 
well  as  information  concerning  the  operator's  velocity  requests  and 
the  vehicle  state.  That  information  is  received  from  the  sixth  board, 
which  is  responsible  for  communicator  with  the  vehicle  control  computers; 
it  receives  the  operator  requests  and  vehicle  state  information  and 
transmits  vehicle  body  and  leg  motion  commands  derived  from  the  deceleration 
plans. 
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The  approach  used  by  the  system's  algorithms  is  to  maintain 
at  all  times  a  body  trajectory  and  leg  motion  sequence  by  which  the 
vehicle  can  be  brought  to  a  halt  in  a  position  of  static  stability. 

That  is,  at  every  point  along  its  path,  the  vehicle  has  available  a 
plan  for  a  vehicle  trajectory  that  will  bring  it  to  a  halt,  together 
with  a  sequence  of  leg  motions  that  will  allow  it  to  follow  that  trajectory 
and  will  leave  it  in  a  state  of  static  stability  at  the  end  of  the  trajectory. 

The  maintenance  of  such  a  trajectory  clearly  places  limits 
on  vehicle  velocity  over  a  given  terrain.  Thus,  with  this  approach, 
operator  requests  for  vehicle  velocities  are  not  always  met,  although 
the  vehicle  will  follow  those  requests  as  closely  as  possible  while 
still  maintaining  vehicle  safety.  It  can  be  seen  that  this  approach 
does  not  permit  an  inexperienced  operator  to  get  the  vehicle  in  trouble, 
but  it  still  allows  an  experienced  operator  to  use  the  full  capabilities 
of  the  vehicle. 


Foot-Lift  Safety  Valve  Task 

This  task  was  originally  intended  to  include  a  revision  of 
the  Battelle  design  of  the  foot-lift  circuit  to  incorporate  experimental 
data.  However,  due  to  a  change  in  the  system  requirements,  a  different 
design  was  used  and  the  Battelle  design  was  never  experimentally  evaluated. 

The  original  scope  also  included  a  review  of  the  design  from 
a  safety  standpoint.  This  task  was  expanded  to  the  design,  fabrication, 
and  testing  of  safety  valve  for  the  foot-lift  circuit.  This  safety 
valve  is  intended  as  a  last-resort  mechanism  for  minimizing  or  eliminating 
any  potential  for  damage  to  either  the  operator  or  the  vehicle  itself. 

The  requirements  for  the  design  of  the  safety  valve  were 
generated  through  discussions  with  OSU.  Based  on  these  requirements, 
several  conceptual  approaches  were  generated  and  evaluated.  After  further 
discussions  with  OSU  personnel,  an  approach  that  involved  electrically- 
actuated  explosive  primer  was  selected.  The  valve  was  then  designed 
and  fabricated  at  Battelle.  Tests  were  then  conducted  to  check  the 
strength  and  integrity  of  the  design  under  pressure,  as  well  as  its 
speed  of  response. 
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FOREWARD 
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1.0  INTRODUCTION 


It  has  long  been  recognized  that  most  man-made  vehicles  are 
greatly  inferior  to  human  beings  and  other  terrestrial  animals  in  off¬ 
road  locomotion.  The  shortcomings  of  current  vehicles  are  particularly 
noticeable  in  the  area  of  mobility.  On  rough  terrain,  a  vehicle  with 
a  passive  suspension  system  must  accomodate  obstacles  by  gross  body 
motions.  On  the  other  hand,  a  system  with  active  suspension  units, 
such  as  legs,  can  pick  its  way  through  rough  terrain  by  selecting  the 
most  suitable  footholds  and  stepping  over  obstacles  and  soft  spots. 

In  addition,  a  legged  system  can  compensate  for  terrain  irregularities 
on  which  it  must  step  by  actively  adjusting  leg  lengths,  thus  providing 
a  much  smoother  ride.  This  report  describes  work  performed  in  this 
project  as  part  of  a  program  to  develop  such  a  vehicle. 

The  first  part  of  the  report  describes  the  development  of 
the  vehicle  "guidance"  system,  which  uses  information  from  a  terrain-sensing 
system  in  the  determination  of  appropriate  vehicle  trajectories  and 
leg  motions.  The  description  of  the  guidance  system  has  three  main 
sections.  The  first  of  those  sections  describes  the  vehicle  and  terrain¬ 
sensing  system  with  which  the  algorithms  are  intended  to  be  used.  The 
second  section  presents  a  description  of  the  algorithms  used  in  the 
system  for  conversion  of  the  terrain  scanner  data  to  an  elevation  map 
and  for  generation  of  plans  for  bringing  the  vehicle  safely  to  a  halt. 

The  last  major  section  of  the  first  portion  of  the  report  describes 
the  computer  hardware  on  which  the  guidance  algorithms  are  implemented. 

In  the  experimental  evaluation  of  the  ASV,  procedures  will 
be  developed  to  minimize  the  potential  for  harm  to  the  operator  or  damage 
to  the  vehicle.  As  a  back-up  system,  Battelle  has  designed,  fabricated, 
and  tested  a  safety  valve  that  will  be  activated  in  case  of  a  major 
system  breakdown.  This  effort  is  described  in  the  second  portion  of 
this  report. 
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2.0  GUIDANCE  SYSTEM  RESEARCH 


2.1  Introduction 


This  portion  of  this  report  discusses  the  development  of  a 
computer  system  that  determines  the  body  and  leg  motions  required  for 
a  legged  adaptive-suspension  vehicle  to  walk  over  rough  terrain.  The 
first  section  describes  the  vehicle  and  its  terrain-sensing  system. 

The  second  section  gives  a  description  of  the  algorithms  used  in  the 
system  for  conversion  of  the  terrain  scanner  data  to  an  elevation  map 
and  for  generation  of  plans  for  vehicle  body  trajectories  and  leg  motion 
sequences.  The  hardware  that  is  used  to  implement  those  algorithms 
is  de_scribed  in  the  third  section.  Finally,  some  conclusions  from  this 
research  are  presented  at  the  end  of  this  portion  of  the  report. 


2.2  Background 

2.2.1  The  Adaptive-Suspension  Vehicle 

The  vehicle  for  which  the  guidance  system  has  been  developed 
is  approximately  15  feet  (4.6  meters)  long  and  4  feet  (1.2  meters)  wide. 

Its  height  can  be  varied  between  approximately  5  feet  (1.5  meters)  and 
9  feet  (2.7  meters)  by  changing  the  extension  of  its  six  three-degree-of- 
freedom  legs.  The  legs  are  attached  at  the  top  of  the  vehicle,  with 
one  pair  each  near  the  front,  middle,  and  rear  of  the  vehicle. 

The  velocity  of  the  vehicle  is  expected  to  be  limited,  at 
least  on  the  rough  terrain  for  which  the  guidance  system  has  been  developed, 
to  a  translational  velocity  of  no  more  than  8  feet/second  (2.4  meters/second) 
and  a  rotational  velocity  of  no  more  than  30  degrees/second.  Translational 
and  rotational  accelerations  are  expected  to  be  limited  to  no  more  than 
4  feet/second/second/  (1.2  meters/second/second)  and  15  degrees/second/second, 
respectively. 
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2.2.2  The  Terrain-Sensing  System 

The  terrain-sensing  system  with  which  the  guidance  system 
described  in  this  report  is  intended  to  work  is  a  scanning  system  mounted 
at  the  front  top  of  the  vehicle.  The  system  scans  both  in  elevation 
and  azimuth,  so,  for  each  scan,  it  provides  information  for  a  sector 
of  terrain  in  front  of  the  vehicle.  For  each  point  in  its  scan,  the 
sending  system  measures  the  distance  from  the  scanner  to  the  terrain 
at  its  current  elevation  and  azimuth  angles. 

2.3  Guidance  System  Algorithms 


2.3.1  Overview 


As  mentioned  above,  the  algorithms  for  the  guidance  system 
perform  two  distinct  tasks.  One  of  those  tasks  is  the  conversion  of 
data  from  the  terrain  scanner  to  a  terrain  elevation  map.  The  other 
is  the  generation  of  vehicle  body  trajectories  and  leg  motion  sequences 
using  the  elevation  map,  the  operator's  vehicle  velocity  requests,  and 
information  concerning  the  vehicle's  state.  The  algorithms  that  perform 
those  two  tasks  are  described  in  the  next  two  sections. 

2.3.2  Elevation  Map  Algorithms 

As  described  above,  the  terrain-sensing  system  provides,  for 
each  of  its  scan  points,  information  on  the  range  to  the  terrain.  Since 
the  scanner  is  fixed  to  the  vehicle,  when  the  vehicle  is  moving,  each 
of  the  scan-point  range  measurements  is  made  from  a  different  position. 
Thus,  the  input  from  the  scanning  system  to  the  guidance  system  consists 
of  scan-point  range  data  indexed  by  elevation  and  azimuth  angles  and 
measured  with  respect  to  the  moving  vehicle. 

However,  the  form  of  terrain  information  most  useful  for  the 
vehicle  guidance  algorithms  is  that  of  elevations  indexed  by  horizontal 
positions.  For  that  reason,  when  a  range  value  is  received  from  the 
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scanner,  the  elevation  map  algorithms  use  the  elevation  and  azimuth 
angles  of  the  range  value,  together  with  the  known  position  and  orientation 
of  the  vehicle,  to  calculate  the  position  in  earth-fixed  Cartesian  co¬ 
ordinates  of  the  point  indicated  by  the  scanner. 

The  terrain  point  positions  in  Cartesian  coordinates  are  then 
stored  in  an  array  of  elevation  values  divided  into  cells  in  a  horizontal 
plane.  The  algorithms  first  determine  whether  there  is  a  cell  present 
in  the  array  for  the  horizontal  position  of  the  terrain  point.  If  so, 
the  elevation  value  for  the  point  is  stored  in  that  cell;  if  not,  a 
cell  is  assigned  to  the  horizontal  position  of  the  point  for  storage 
of  the  elevation  value.  Since  the  terrain  array  is  fixed  in  size,  this 
method  requires  that,  as  the  vehicle  moves,  data  storage  "wrap  around" 
from  one  portion  of  the  array  to  another.  Thus,  areas  of  the  terrain 
are  automatically  "forgotten"  after  the  vehicle  has  passed  some  distance 
beyond  them. 

2.3.3  Vehicle  Guidance  Algorithms 

The  function  of  the  vehicle  guidance  algorithms  is  to  determine 
appropriate  body  and  leg  motion  commands  for  the  vehicle  control  system, 
based  on  current  operator  requests.  Those  operator  requests  can  be 
for  three  components  of  vehicle  velocity:  forward,  side  (crab),  and 
turning  (yaw).  The  guidance  algorithms  attempt  to  match  the  vehicle 
velocity  to  the  operator's  requests  as  closely  as  possible;  however, 
as  discussed  below,  those  requests  are  not  always  attainable,  due  to 
considerations  of  vehicle  stability. 

Since  the  three  vehicle  velocity  components  mentioned  above 
are  usually  the  only  ones  which  are  of  direct  interest  to  the  operator, 
the  vehicle  guidance  algorithms  automatically  control  the  vehicle  eleva¬ 
tion,  pitch,  and  roll  based  on  the  terrain  over  which  the  vehicle  is 
passing.  Then,  once  all  six  vehicle  velocity  components  are  specified, 
the  guidance  algorithms  determine  the  leg  motions  required  to  attain 
those  velocities  while  maintaining  vehicle  stability.  The  information 
required  by  the  guidance  algorithms  to  provide  these  body  and  leg  motion 
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commands  includes,  in  addition  to  the  operator's  requests  and  the  terrain 
elevation  map  described  in  the  previous  section,  information  concerning 
the  current  vehicle  body  state  (position  and  velocity)  and  the  positions 
and  support  states  of  the  legs,  as  well  as  knowledge  of  the  limitations 
on  vehicle  velocity  and  acceleration  and  on  leg  motions. 

The  use  of  that  information  in  the  operation  of  the  guidance 
algorithms  is  discussed  in  the  following  three  parts  of  this  section. 

The  first  part  presents  the  general  principles  on  which  the  algorithms 
are  based  and  describes  the  operation  of  the  algorithms'  highest  level, 
which  determines  how  closely  the  vehicle  can  follow  the  operator's  velocity 
requests.  The  second  part  gives  a  description  of  the  generation  of 
trajectories  for  the  vehicle  body,  based  on  the  operator's  requests. 
Finally,  the  last  part  describes  the  generation  of  sequences  of  leg 
motions  that  allow  the  vehicle  body  to  follow  those  trajectories. 

2. 3. 3.1  Vehicle  Guidance  Algorithm  Approach.  Previous  approaches 
to  the  problem  of  legged  vehicle  locomotion  have  emphasized  the  concept 
of  static  stability,  the  condition  in  which  the  vertical  projection 
of  the  vehicle's  center  of  mass  is  within  the  convex  polygon  formed 
by  the  vertical  projections  of  those  of  the  vehicle's  feet  that  are 
on  the  ground.  Those  earlier  approaches  have  used  as  their  goal  in 
vehicle  control  the  maintenance  of  the  vehicle  in  a  state  of  static 
stability  at  all  times.  This  method  of  control  has  been  possible  because 
vehicle  dynamics  have  been  relatively  unimportant  since  vehicle  speeds 
have  been  quite  low. 

However,  for  a  vehicle  operating  at  higher  speeds,  maintenance 
of  static  stability  is  not  sufficient  for  vehicle  security,  since  in 
many  cases  the  vehicle's  motion  could  carry  its  center  of  mass  outside 
its  support  polygon.  Thus,  a  faster  vehicle  requires  some  sort  of  dynamic 
control  of  vehicle  stability  to  ensure  its  safety.  (Of  course,  for 
those  times  when  the  vehicle  is  not  moving,  static  stability  is  still 
sufficient. ) 

The  approach  that  is  used  by  the  system  described  in  this 
report  is  to  maintain  at  all  times  a  plan  by  which  the  vehicle  could 
be  brought  to  a  halt  in  a  position  of  static  stability.  That  is,  at 
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every  point  along  its  path,  the  vehicle  has  available  a  plan,  for  a  vehicle 
trajectory  that  would  bring  it  to  a  halt,  together  with  a  sequence  of 
leg  motions  that  would  allow  it  to  follow  that  trajectory  and  would 
leave  it  in  a  state  of  static  stability  at  the  end  of  the  trajectory. 

The  maintaining  of  such  a  trajectory  clearly  places  limits 
on  vehicle  velocity  over  a  given  terrain.  Thus,  with  this  approach, 
operator  requests  for  vehicle  velocities  are  not  always  met,  although 
the  vehicle  follows  those  requests  as  closely  as  possible  while  still 
maintaining  vehicle  safety.  The  task  of  determinig  how  closely  the 
vehicle  can  follow  the  operator's  velocity  requests  is  performed  by 
the  highest  level  of  the  guidance  system's  algorithms. 

The  algorithms  make  that  determination  as  shown  in  the  flow 
chart  in  Figure  1  (flow  chart  conventions  for  this  report  are  given 
in  Appendix  A).  They  first  evaluate  whether  it  is  possible,  for  acceler¬ 
ations  that  would  allow  the  vehicle  to  attain  the  operator's  current 
velocity  requests  as  soon  as  possible,  to  calculate  a  vehicle  body  trajectory 
and  leg  motion  sequence  that  would  allow  the  vehicle  to  be  brought  to 
a  halt  safely.  If  so,  the  body  and  leg  motion  commands  to  accelerate 
the  vehicle  in  the  direction  of  the  operator's  requests  are  sent  to 
the  vehicle  control  system.  If  not,  the  same  attempt  (to  find  a  body 
trajectory  and  leg  motion  sequence  that  would  bring  the  vehicle  safely 
to  a  halt)  is  made  for  each  of  several  successively  smaller  vehicle 
accelerations  in  the  direction  of  the  operator's  velocity  requests. 

If  a  body  trajectory  and  leg  motion  sequence  for  halting  are  found  for 
any  of  those  vehicle  acceleration  (compromise)  selections,  then  the 
comands  to  implement  that  selection  are  sent  to  the  vehicle  control 
system. 

If  no  part  of  the  operator's  velocity  requests  can  be  executed 
safely,  then  the  guidance  system  derives  the  commands  for  the  vehicle 
control  system  from  the  most  recently  generated  body  trajectory  and 
leg  motion  sequence.  That  is,  since  the  operator  is  requesting  that 
the  vehicle  accelerate  to  unsafe  velocities  (unsafe  because  the  vehicle 
could  not  be  brought  safely  to  a  halt  if  it  were  accelerated  toward 
those  velocities),  the  guidance  system  must  ignore  the  operator's  requests 
and  derive  its  commands  from  a  plan  that  it  knows  to  be  safe,  the  body 
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FIGURE  1.  VEHICLE  GUIDANCE  ALGORITHM  HIGH-LEVEL  FLOW  CHART 
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trajectory  and  leg  motion  sequence  that  it  previously  determined  would 
bring  the  vehicle  safely  to  a  halt. 

Thus,  both  the  safety  and  the  performance  of  the  vehicle  are 
dependent  on  the  guidance  system's  generation  of  appropriate  body  trajec¬ 
tories  and  leg  motion  sequences  for  the  vehicle.  The  next  two  sections 
describe  the  methods  which  the  system  uses  to  generate  those  body  trajec¬ 
tories  and  leg  motion  sequences. 

2. 3. 3. 2  Vehicle  Body  Trajectory  Generation.  As  mentioned 
earlier,  the  vehicle's  operator  makes  requests  for  forward,  side,  and 
turning  velocities  for  the  vehicle.  Thus,  when  the  guidance  system 
algorithms  attempt  to  generate  a  vehicle  body  trajectory,  the  vehicle's 
desired  forward,  side,  and  turning  accelerations  are  specified;  as  described 
in  the  last  section,  they  may  be  accelerations  that  would  move  the  vehicle 
most  rapidly  toward  the  operator's  requested  velocities  or  a  smaller 
acceleration  "compromise"  selection. 

As  shown  in  Figure  2  (again,  see  Appendix  A  for  flow-chart 
conventions),  the  algorithm  generates  the  first  portion  of  the  vehicle 
path  over  the  terrain  by  assuming  that  the  vehicle  will  accelerate  at 
the  desired  rates  for  the  time  that  passes  between  successive  iterations 
of  the  guidance  algorithms.  The  desired  acceleration  rates  determine 
three  of  the  vehicle's  degrees  of  freedom  (horizontal  position  and  yaw 
angle)  at  discrete  points  on  the  first  portion  of  its  trajectory;  the 
acceleration  rates  are  also  used  to  calculate  the  times  at  which  those 
points  on  the  trajectory  are  reached.  The  algorithm  then  uses  the  terrain 
map  to  determine  the  elevation  and  slope  of  the  terrain  at  the  discrete 
points  along  the  calculated  path.  Then  the  algorithm  uses  the  elevation 
and  slope  information  together  with  the  desired  vehicle  altitude  and 
the  desired  relation  of  vehicle  orientation  to  the  terrain  slope  to 
calculate  the  three  remaining  degrees  of  freedom  (vehicle  elevation, 
pitch,  and  roll).  Thus,  the  first  portion  of  the  body  trajectory  consists 
of  vehicle  positions  (specified  by  the  six  body  degrees  of  freedom) 
at  given  times  for  discrete  points  along  the  path  determined  by  the 
desire  accelerations. 
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FIGURE  2.  VEHICLE  BODY  TRAJECTORY  GENERATION  FLOW  CHART 
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The  second  and  final  portion  of  the  vehicle  body  trajectory 
is  generated  in  a  similar  manner.  The  vehicle  is  assumed  to  decelerate 
at  its  maximum  rate  while  continuing  on  the  same  path  that  is  was  follow¬ 
ing  at  the  end  of  the  first  portion  of  the  trajectory.  The  deceleration 
rates  are  used  to  determine  the  vehicle's  horizontal  position  and  yaw 
angle,  together  with  the  associated  times,  for  points  along  the  second 
portion  of  the  trajectory.  The  elevation  map  is  then  used  in  the  calculation 
of  the  other  three  degrees  of  freedom  for  those  points  on  the  trajectory. 

If  at  any  point  in  the  generation  of  the  first  or  second  portion 
of  the  body  trajectory  the  algorithm  cannot  obtain  sufficient  data  from 
the  terrain  elevation  map  to  determine  the  terrain  elevation  and  slope, 
the  attempt  at  trajectory  generation  is  aborted,  which  implies  that 
the  vehicle  cannot  implement  the  desired  accelerations.  Otherwise, 
when  the  trajectory  generation  is  complete,  the  guidance  system  proceeds, 
as  described  in  the  next  section,  to  attempt  to  determine  a  leg  motion 
sequence  which  will  enable  the  vehicle  to  follow  the  calculated  trajectory. 

2. 3. 3. 3  Vehicle  Leg  Motion  Sequence  Generation.  The  approach 
to  the  generation  of  vehicle  leg  motion  sequences  is  shown  in  Figure 
3  (see  Appendix  A  for  flow-chart  conventions).  The  algorithm  uses  an 
iterative  approach  that  proceeds  either  until  a  leg  motion  sequence 
is  generated  for  the  complete  body  trajectory  (which  brings  the  vehicle 
to  a  halt)  or  until  a  point  is  reached  at  which  no  acceptable  continuation 
of  the  leg  motion  sequence  can  be  found.  The  iterations  of  the  algorithm 
take  place  at  the  successive  discrete  points  of  the  previously  generated 
vehicle  body  trajectory. 

For  each  iteration  of  the  algorithm,  the  first  step  is  to 
determine,  using  the  already-generated  portion  of  the  leg  motion  sequence, 
whether  at  that  point  in  the  body  trajectory,  if  the  motion  sequence 
were  executed,  any  of  the  legs  would  be  completing  their  transfers  from 
earlier  footholds  to  new  ones.  If  so,  it  is  assumed  that  those  legs 
would  be  supporting  the  vehicle  body  at  that  point  in  its  trajectory. 

The  vehicle's  static  stability  is  then  calculated  using  the  information 
of  the  positions  of  the  vehicle's  legs  that  would  be  supporting  its 


FOR  EACH  POINT  ON 
VEHICLE  BODY  TRAJECTORY. 
OR  UNTIL  IT  IS  DETERMINED 
THAI  A  LEG  MOTION  SEQUENCE 
CAN  NOT  BE  GENERATED 
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FIGURE  3.  VEHICLE  LEG  MOTION  SEQUENCE  GENERATION  FLOW  CHART 
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body  at  that  point,  together  with  the  position  of  the  vehicle  body  at 
that  point  in  the  body  trajectory.  If  the  vehicle  would  be  statically 
unstable  at  that  point,  the  generation  of  the  leg  motion  sequence  is 
aborted;  if  the  vehicle  would  be  stable,  the  motion  sequence  is  aborted; 
if  the  vehicle  would  be  stable,  the  motion  sequence  generation  continues 
as  described  in  the  following  paragraphs. 

The  next  step  in  the  iteration  is  to  calculate,  for  those 
legs  that  would  be  supporting  the  vehicle,  the  legs'  kinematic  margins, 
which  are  the  distances  along  the  body  trajectory  over  which  the  vehicle 
could  move  until  the  legs  reach  their  kinematic  limits.  If  any  of  the 
legs  would  have  a  kinematic  margin  of  less  than  zero  at  the  point  on 
the  trajectory  that  is  being  considered  (that  is,  if  any  of  the  legs 
would  be  out  of  its  limits  when  the  vehicle  was  at  that  position),  then 
the  generation  of  the  leg  motion  sequence  is  aborted. 

Otherwise,  the  algorithm,  beginning  with  the  leg  with  the 
lowest  kinematic  margin  and  continuing  to  that  with  the  highest,  determines 
if  any  of  the  supporting  legs  could  be  moved.  It  does  that  by  first 
evaluating  whether  the  legs  could  be  lifted  while  leaving  the  vehicle 
stable.  If  it  could,  the  algorithm  then  attempts  to  find  an  appropriate 
foothold  to  which  the  leg  could  be  moved;  if  it  finds  an  acceptable 
foothold,  it  adds  to  the  leg  motion  sequence  the  plan  to  move  the  leg 
to  that  foothold.  If  no  foothold  is  found,  or  if  the  algorithm  determined 
that  the  leg  could  not  be  lifted,  the  attempt  to  move  a  leg  is  terminated. 

The  procedure  described  in  the  proceeding  paragraphs  is  performed 
at  each  point  along  the  vehicle  body  trajectory.  If  at  any  point  along 
the  trajectory  the  procedure  is  aborted,  the  vehicle  is  not  able  to 
implement  the  accelerations  that  were  used  to  generate  the  body  trajectory. 
Otherwise,  when  the  last  iteration  of  the  procedure  at  the  last  point 
on  the  trajectory  is  performed  successfully,  the  leg  motion  sequence 
for  the  body  trajectory  is  complete. 
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2.4  Guidance  System  Hardware 


2.4.1  Overview 


A  diagram  of  the  overall  structure  of  the  guidance  system 
hardware  and  its  internal  and  external  communication  channels  is  shown 
in  Figure  4.  It  can  be  seen  there  that  for  its  external  communication 
the  system  receives  information  from  the  terrain-scanning  system  and 
both  sends  information  to  and  receives  it  from  the  vehicle  control  system. 
As  can  also  be  seen  in  the  figure,  nearly  all  of  the  guidance  system's 
internal  inter-board  communication  takes  place  over  the  Intel  Multibus, 
although  in  one  case  parallel  data  lines  between  boards  are  used. 

Four  of  the  six  boards  in  the  system  are  Intel  i SBC  86/30 
microprocessor  boards,  two  of  which  contain  an  Intel  8087  numeric  data 
processor  as  well  as  the  Intel  8086  processor  with  which  the  boards 
are  normally  equipped.  One  of  the  other  two  boards  in  the  system  contains 
special-purpose  circuitry  designed  to  receive  the  data  from  the  terrain¬ 
scanning  system.  The  sixth  board  is  a  512-kilobyte  memory  board  in 
which  the  terrain  elevation  map  is  scored. 

As  shown  in  Figure  4,  the  six  boards  can  be  divided  into  three 
subsystems.  One  of  the  subsystems  consists  of  a  single  processor  which 
performs  the  tasks  required  for  the  communication  with  the  vehicle  control 
system.  The  other  two  subsystems  perform  the  elevation  map  processing 
and  the  vehicle  guidance  processing;  they  communicate  only  through  the 
terrain  elevation  map.  The  three  subsystems  are  described  in  the  following 
three  parts  of  this  section. 

2.4.2  Elevation  Map  Subsystem 

The  elevation  map  subsystem  receives  terrain  range  data  from 
the  terrain  scanner,  converts  those  data  to  terrain  elevations  in  Cartesian 
coordinates,  and  then  stores  the  elevations  in  a  terrain  elevation  map. 
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FIGURE  4.  VEHICLE  GUIDANCE  SYSTEM  COMPUTER  CONFIGURATION 
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Two  boards  are  used  to  perform  the  processing  needed  for  the  coordinate 
conversion  and  storage  of  the  data  so  that  the  calculations  for  each 
range  value  can  be  performed  very  rapidly.  However,  even  with  the  two 
processor  boards,  the  scanner  sends  data  faster  than  the  calaulations 
can  be  performed,  so  a  third  board  was  designed  and  constructed  to  select 
only  a  given  portion  of  the  scanner  data  to  pass  to  the  processor  boards. 

Thus,  the  elevation  map  subsystem  consists  of  one  special-purpose 
circuit  board  and  two  Intel  i SBC  86/30  processor  boards.  The  operations 
of  the  three  boards  are  described  in  the  following  paragraphs. 

2. 4. 2.1  Scanner  Data  Selection  Circuit.  The  scanner  data 
selection  board  (the  board  layout  and  circuit  diagram  of  which  are  shown 
in  Appendices  B  and  C,  respectively)  receives  over  parallel  lines  from 
the  terrain-scanning  system  data  consisting  of  the  terrain  range  values 
and  the  line  numbers  in  the  raster-style  scan  for  those  range  values. 

The  board  also  has  one  memory  location  on  the  Multibus  into  which  the 
scanner  data  conversion  processor  can  write  a  number  from  one  to  fifteen. 
That  number  in  the  memory  location  is  used  by  the  circuit  to  determine 
what  proportion  of  the  range  data  should  be  passed  to  the  data  conversion 
processor:  for  the  number  n,  every  nth  datum  in  every  nth  row  of  data 

is  passed  to  the  conversion  processor. 

In  operation,  the  board  receives  every  range  datum  from  the 
scanner  and  then  acknowledges  that  reception  to  the  scanner.  However, 
until  a  number  is  written  into  the  board's  memory,  none  of  the  data 
is  passed  on  to  the  scanner  data  conversion  processor.  When  a  number 
is  written  into  the  memory,  the  board  begins  its  normal  operation,  in 
which  the  data  to  be  passed  to  the  conversion  processor  are  determined 
as  described  in  the  preceding  paragraph.  Those  data  that  are  to  be 
passed  are  sent  over  parallel  lines  to  the  conversion  processor  board, 
which  is  described  in  the  next  section. 

2. 4. 2. 2  Scanner  Data  Conversion  Processor.  The  scanner  data 
conversion  board,  the  computer  program  listings  of  which  are  shown  in 
Appendix  D,  receives  the  scanner  range  data  over  parallel  lines,  as 
described  in  the  preceding  section.  In  addition,  it  receives  frequent 
information  over  the  Multibus  from  the  vehicle  control  communication 
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board  concerning  the  current  position  and  orientation  of  the  vehicle. 

It  uses  that  information  as  described  in  Section  2.3.2  to  convert  the 
'•canner  range  data  to  terrain  elevations.  It  then  sends  those  elevation 
values  over  the  Multibus  to  a  buffer  on  the  elevation  map  storage  processor 
board  which  is  described  in  the  next  section. 

2. 4. 2. 3  Elevation  Map  Storage  Processor.  The  elevation  map 
storage  board,  the  program  listings  of  which  are  shown  in  Appendix 
E,  contains  a  buffer  into  which  the  scanner  data  conversion  board  writes 
terrain  elevation  values,  as  described  in  the  preceding  section.  The 
only  task  of  the  map  storage  board  is  to  store  the  elevation  values 
at  the  proper  locations  (determined  as  described  in  Section  2.3.2)  in 
the  elevation  map  memory  board.  The  elevation  map  can  then  be  used 
by  the  vehicle  guidance  subsystem,  which  is  described  in  the  next  section. 

2.4.3  Vehicle  Guidance  Subsystem 

The  purpose  of  the  single  Itel  i SBC  86-30  processor  board 
that  comprises  the  vehicle  guidance  subsystem  is  to  generate,  with  the 
computer  programs  listed  in  Appendix  F,  the  vehicle's  body  trajectories 
and  leg  motion  sequences.  For  that  purpose,  it  receives,  over  the  Multibus 
from  the  vehicle  control  communication  board,  information  concerning 
operator  velocity  requests,  current  vehicle  body  state  (position  and 
velocity),  and  the  positions  and  support  states  of  the  legs.  The  guidance 
board  uses  that  information,  together  with  the  elevation  map  on  the 
memory  board,  to  generate  the  body  trajectories  and  leg  motion  sequences 
as  described  in  Section  2.3.3.  It  stores  the  trajectories  and  motion 
sequences  in  a  buffer  on  the  board  itself,  where  the  information  in 
them  can  be  accessed  by  the  vehicle  control  communication  subsystem, 
which  is  described  in  the  next  section. 

2.4.4.  Vehicle  Control  Communication  Subsystem 

The  vehicle  control  communication  subsystem,  which  consists 
of  one  Intel  i SBC  86/30  processor  board  using  the  programs  listed  in 
Appendix  G,  is  responsible  for  all  communications  between  the  guidance 
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system  and  the  vehicle  control  system.  The  communication  subsystem 
periodically  receives,  over  parallel  lines  from  the  vehicle  control 
system,  the  operator's  vehicle  velocity  requests,  the  current  vehicle 
body  state  (position  and  velocity),  and  the  positions  and  support  states 
of  the  vehicle's  legs.  It  then  passes  all  that  information  over  the 
Multibus  to  the  guidance  subsystem.  In  addition,  the  communication 
board  sends  vehicle  position  information  over  the  Multibus  to  the  scanner 
data  conversion  board;  since,  to  provide  accurate  conversion  of  the 
scanner  data,  the  conversion  board  requires  new  vehicle  position  information 
more  frequently  than  it  is  provided  by  the  vehicle  control  system,  the 
communication  board  interpolates  between  the  vehicle  positions  provided 
by  the  control  system  by  asking  the  vehicle  velocity  information  that 
it  also  receives  from  the  control  system. 

Finally,  the  communication  board  also  provides  the  vehicle 
body  and  leg  motion  commands  to  the  vehicle  control  system.  It  derives 
those  commands  from  the  body  trajectories  and  leg  motion  sequences  stored 
in  the  buffer  on  the  vehicle  guidance  board,  and  it  sends  the  commands 
over  parallel  data  lines  to  the  control  system. 

2.5  Conclusion 


In  summary,  this  portion  of  the  report  describes  a  computer 
system  developed  to  enable  a  legged  vehicle  to  walk  over  rough  terrain. 

The  system  uses  data  from  a  terrain-scanning  system,  information  from 
the  vehicle's  control  system,  and  knowledge  of  the  vehicle's  capabilities 
and  limitations  to  determine  the  body  and  leg  motions  required  for 
the  vehicle's  locomotion  over  the  terrain.  The  system  has  been  extensively 
tested  with  the  terrain  scanner  and  with  a  breadboard  version  of  the 
vehicle  control  computer,  and  it  will  soon  be  installed  in  the  vehicle 
itself. 
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3.0  FOOT-LIFT  SAFETY  VALVE  RESEARCH 


3.1  Background  and  Valve  Operation 

In  the  development  of  any  complex  system  such  as  the  A$V, 
fail-safe  measures  must  be  incorporated  into  the  system  design  to  mini¬ 
mize  or  eliminate  the  possibility  of  a  catastrophic  system  failure, 
i.e.,  one  resulting  in  substantial  damage  to  the  machine  or  harm  to 
the  operator.  Such  failures  could  result  from  failure  of  one  or  more 
of  the  sensor  systems,  a  computer  malfunction,  or  unanticipated  terrain. 

These  fail-safe  mechanisms  are  primarily  intended  as  a  back¬ 
up  to  the  first  two  safety  mechanisms: 

1)  Proper  operation  and  maintenance  of  the  ASV. 

2)  Experimental  procedures  designed  to  minimize  any  risk 
to  the  machine  or  the  operator. 

3.2  Valve  Operation 

In  discussion  with  program  personnel  at  Ohio  State  University, 
it  was  agreed  that  some  type  of  fail-safe  mechanism  should  be  designed 
into  the  hydraulic  systems  for  the  legs  of  the  ASV.  Two  alternative 
approaches  were  considered.  The  first  approach  involved  a  safety  valve 
that  would  connect  the  pressure  side  of  the  foot-lift  circuit  to  the 
return  side,  in  parallel  with  the  actuator.  Both  sides  of  the  actuator 
would,  in  effect,  be  connected  together.  As  a  result,  there  would  be 
no  pressure  differential  across  the  actuator,  and  the  actuator  would 
not  support  any  load.  The  vehicle  leg  would,  in  effect,  "go  limp"  and 
the  vehicle,  driven  by  its  own  weight,  would  do  a  "belly  flop".  A  res¬ 
triction  in  the  safety  valve  would  provide  some  control  to  this  dropping 
action  of  the  vehicle  and  would  dissipate  some  of  the  vehicle's  energy 
before  it  hit  the  ground.  However,  this  would  also  reduce  the  speed 
of  leg  response. 

A  second  approach  was  to  install  a  similar  safety  valve  that 
would  connect  the  pressure  side  with  the  return  side  in  place  of  the 
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actuator.  In  this  approach,  the  vehicle  legs  are  "frozen"  in  place. 
This  is  accomplished  by  blocking  off  each  end  of  the  actuator,  and  pre¬ 
venting  the  flow  of  fluid  out  of  either  end.  Each  leg  will  act  like 
a  rigid  structural  member  connected  to  the  mainframe.  This  will  prob¬ 
ably  result  in  a  strong,  jerking  motion,  and  inertial  forces  may  tend 
to  roll  the  vehicle. 

In  some  situations,  the  "belly  flop"  approach  was  felt  to 
be  the  safest  approach,  while  other  situations  seemed  to  require  the 
second  alternative.  To  accomodate  this  double  requirement,  a  system 
was  designed  that  was  capable  of  providing  either  alternative.  A  sche¬ 
matic  representation  of  this  system  is  shown  in  Figure  5.  If  only  the 
dump  valve  is  actuated,  the  vehicle  will  drop.  If  both  the  dump  and 
the  block  valves  are  actuated,  the  vehicle  legs  will  be  locked  up. 

Operation  of  the  walking  machine  is  so  complex  that  the  op¬ 
erator  will, not  be  able  to  react  quickly  enough  to  manually  actuate 
the  valves.  Therefore,  the  system  was  designed  for  operation  by  the 
vehicle -computer  through  a  established  preprogrammed  procedure.  Depend 
ing  on  the  circumstances,  the  computer  may  dump  some  of  the  legs  and 
keep  others  stiff  to  control  where  and  how  the  machine  falls.  This 
would  be  useful  if  it  were  necessary  to  avoid  an  object  while  falling. 
Computer  control  could  also  be  used  to  prevent  the  walking  machine  from 
rolling  over.  The  actual  safety  algorithms  are  to  be  developed  by  Ohio 
State  University  at  a  future  date. 

3.3  Valve  Design 

The  safety  valve  was  required  to  handle  the  full  output  flow 
of  the  pump  (30  GPM).  Since  the  hydrostatic  circuit  for  each  leg  ac¬ 
tuator  is  relatively  short,  the  flow  losses  through  the  valve  must  be 
minimized  to  control  the  temperature  rise  of  the  hydraulic  fluid.  Flow 
losses  through  the  valve  were  to  be  limited  to  approximately  30  psi. 
Finally,  the  overall  package  was  to  be  as  small  as  possible  to  allow 
it  to  fit  into  the  leg  assembly. 
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FIGURE  5.  HYDRAULIC  SCHEMATIC  OF  SAFETY  VALVE  CIRCUIT 
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Several  different  packaging  alternatives  were  considered. 

One  option  was  to  mount  a  single  valve  directly  on  the  pump  output  ports. 
A  second  option  was  to  machine  an  internal  valve  in  the  crossover  relief 
valve  on  the  pump.  The  third  alternative  was  a  separate  valve  block 
with  all  three  functions  in  it.  The  alternatives  were  evaluated,  in 
conjunction  with  OSU,  and  the  third  option,  the  separate  valve  block, 
was  selected. 


3.3.1  Valve  Actuation 


Unless  major  problems  with  the  system  develop  during  test¬ 
ing,  the  actuation  of  the  safety  valve  will  be  very  infrequent.  How¬ 
ever,  when  the  valve  does  operate,  it  must  do  so  with  high  reliability. 
Solenoid-actuated  devices  were  not  felt  to  have  sufficient  reliability 
for  this  application.  On  the  other  hand  electrically-actuated  explosive 
device  such  as  explosive  bolts,  have  demonstrated  high  reliability, 
even  after  being  installed  for  weeks  or  months.  The  major  drawback 
with  these  devices  is  that  they  are  not  reuseable;  a  new  one  must  be 
installed  after  each  use.  Any  explosive  actuator  chosen  must  therefore 
be  relatively  inexpensive.  Since  explosive  bolts  cost  in  the  range 
of  100  dollars  each.,  they  were  considered  to  be  too  expensive  for  this 
application.  A  lower  cost  alternative,  electrically-fired  primers  (Olin 
BWP-8-4-257W)  were  found  to  offer  high  reliability  at  a  reasonable  cost. 
These  devices  could  easily  be  adapted  to  actuate  the  safety  valve  with 
a  simple  spring-activation  technique. 

3.3.2  Valve  Design 

The  valve  layout  can  be  seen  in  Figure  6,  a  photograph  of 
the  valve.  The  valve  body  has  three  parallel  spools  running  completely 
through  the  valve  body.  The  center  spool  is  the  dump  section  and  the 
two  outside  spools  are  the  block  sections  of  the  valve.  The  three  ex¬ 
plosive  cavities  are  at  one  end  of  the  valve  spool  (bottom  of  picture) 
and  their  bolt-on  flanges  are  shown.  At  the  other  end  of  the  valve 


FIGURE  6.  SAFETY  VALVE 
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(top  of  photo),  there  is  a  hex  plug  which  contains  the  compression  spring. 
Access  can  be  gained  to  the  spool  through  either  end  of  the  valve. 

The  side  of  the  valve  contains  the  hydraulic  connection  ports.  The 
ports  are  sized  for  a  SAE-10  straight  threaded  connection.  The  connections 
shown  on  the  left  are  for  a  pump-to-actuator  port  connection.  The  right 
side  would  be  the  same  except  that  the  pump  and  actuator  ports  would 
be  switched.  The  electrical  connections  are  the  thin  wires  visible 
underneath  the  explosive  cavities.  Only  one  common  ground  connection 
is  necessary  for  all  three  explosive  housings. 

3.3.3  Operating  Pressure 

In  the  current  system  concept,  the  leg  actuator  system  pres¬ 
sure  will  range  from  a  minimum  of  150  psi  to  a  maximum  of  4000  psi. 

During  normal  operation  the  pressure  will  be  considerably  less  than 
the  maximum  (  -400-500  psi).  However,  pressure  transients  may  occur 
which  result  in  pressure  spikes  higher  than  the  4000  psi  maximum.  Since 
the  seal  on  the  sliding  spool  shaft  must  be  capable  of  handling  this 
pressure  without  extruding,  back-up  rings  were  placed  on  both  sides 
of  the  seal . 

3.3.4  Operating  Time 

The  valve  was  also  required  to  have  a  quick  operating  time 
so  that  the  safety  procedure  could  be  actuated  with  enough  time  to  be 
effective.  OSU  estimated  that  the  required  time  of  operation  was  50 
milliseconds  (msec)  for  complete  shifting,  once  the  electrical  signal 
had  been  applied.  This  required  the  use  of  a  stiff  spring  to  accelerate 
the  spool  fast  enough.  It  also  required  that  the  explosive  primer  op¬ 
erate  quickly.  (The  published  time  for  primer  operation  is  less  than 
2  msec. ) 
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3.3.5  Spool  Clearance 

In  order  for  the  safety  valve  to  operate  with  high  reliability 
it  is  necessary  to  ensure  that  the  valve  spool  shifts.  The  major  cause 
of  spool  hang-up  is  radial  forces  applied  to  the  spool  due  to  uneven 
pressure  distribution.  These  radial  forces  cause  friction  between  the 
valve  wall  and  the  spool  preventing  the  spool  from  moving.  The  primary 
way  to  avoid  these  forces  is  to  eliminate  any  pressure  drops  acting 
on  on  the  side  of  the  spool.  To  accomplish  this,  the  counterbores  around 
the  ports  were  designed  as  large  as  possible  so  that  the  pressure  around 
the  spool  is  equal  in  all  radial  directions.  Equal  pressure  around 
the  spool  avoids  any  radial  forces  caused  by  fluid  flow. 

Although  it  is  important  that  there  be  enough  valve  body/spool 
clearance  to  allow  free  movement  of  the  spool,  the  larger  the  clear¬ 
ance,  the  greater  the  valve  leakage.  The  leakage  rate  must  be  minimized 
to  assure  proper  system  performance.  The  leakage  rate  of  the  spool 
can  be  determined  by  the  equation: 

Q  =  TTr  c3  (P1  -  P2)/2yL 

where 

Q  =  valve  leakage  rate 
c  =  clearance 
r  =  radial  bore 
y  =  viscosity 

Pi  -  P2  =  pressure  drop  across  the  spool 
L  =  length  of  spool  land 

This  is  the  average  rate  of  flow  lost  through  the  dumping 
section  of  the  safety  valve  during  normal  operation.  It  is  small  enough 
to  assure  proper  system  performance. 

Once  the  valve  has  been  actuated  (in  the  "locked  leg"  mode), 
fluid  will  leak  across  the  block  valves.  Since  these  valves  will  normal¬ 
ly  not  see  a  4000  psi  pressure  differential  across  them,  the  leakage 
rate  will  be  less  than  0.091  GPM.  As  the  blocking  valves  leak,  the 
legs  will  slowly  creep  and  the  vehicle  body  will  drop.  However,  the 
rate  will  be  quite  slow  under  these  circumstances  and  is  considered 
acceptable. 


25 


The  forces  acting  along  the  axis  of  the  spool  control  the 
shifting  of  the  valve  spool.  In  order  to  shift  the  spool,  there  must 
be  a  net  force  in  the  direction  of  shifting.  The  valve  is  designed 
so  that  a  force  due  to  an  area  differential  in  the  spool  will  act  in 
conjunction  with  a  spring  to  shift  the  spool.  Opposing  this  movement 
is  spool-wall  friction,  rod-seal  friction  and  flow  losses  moving  fluid 
through  the  spool.  Calculation  of  these  forces  (Appendix  H)  indicates 
that,  even  under  high  pressures,  the  net  force  is  always  positive  in 
the  shifting  direction. 


3.4  Valve  Operation 

Under  normal  operation  of  the  valve,  fluid  from  the  pump  enters 
the  valve  port  and  passes  through  four  1/2-inch  holes  in  one  of  the 
outside  spools  (depending  on  the  direction  of  operation).  The  fluid 
then  flows  axially  along  the  3/4-inch  diameter  bore  in  the  center  of 
the  spool.  At  the  other  end  of  the  spool,  the  fluid  exits  through  a 
second  set  of  ^-inch  diameter  holes.  (For  purposes  of  calculation, 
then,  the  valve  may  be  treated  as  two  orifice  valves,  each  with  four 
1/2-inch  diameter  holes  and  a  section  of  3/4-inch  diameter  pipe  approxi¬ 
mately  4  inches  long.)  The  return  flow  passes  through  the  other  outside 
spool  in  a  similar  manner.  The  center  spool  normally  is  blocked  (the 
offset  holes  in  the  center  spool  are  covered  by  the  valve  body  lands) 
so  that  no  flow  passes  through  it. 

To  activate  the  valve,  a  electrical  signal  from  the  computer/ 
electronics  system  is  applied  to  the  explosive  actuator.  A  signal  pulse 
of  24  volt  D.C.  at  the  actuator  of  at  least  2  msec  in  duration  is  required. 
Voltage  drops  from  the  power  source  to  the  valve  are  not  included  in 
this  value.  The  electrical  connection  to  the  valve  is  through  three 
22  gauge  wires.  The  valve  body  is  grounded  to  the  leg  frame  for  the 
other  electrical  connection. 

Once  the  electrical  signal  is  given  to  the  valve,  the  explo¬ 
sive  actuator  will  detonate.  Gases  produced  by  the  explosion  are  con¬ 
tained  in  the  ceramic  tube,  where  they  build  up  pressure,  rupturing 
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the  tube.  The  ceramic  tube  is  completely  destroyed  by  the  explosion, 
freeing  the  spring  to  push  the  spool  to  its  offset  position.  As  each 
of  the  outside  block  spools  shifts,  the  radial  holes  in  it  are  covered 
by  the  valve  body  lands,  and  flow  is  shut  off.  The  spool,  when  complete¬ 
ly  shifted,  has  moved  5/8  inch.  The  length  of  the  spring  has  been  de¬ 
signed  to  ensure  that  there  is  spring  pressure  holding  the  spool  in 
the  shifted  position.  The  operation  of  the  center  dump  spool  is  iden¬ 
tical  except  that  the  radial  holes  go  from  a  covered  to  an  uncovered 
land  position,  allowing  flow. 

3.5  Valve  Refurbishment 


The  valve  body  contains  an  explosive  cavity  that  was  designed 
for  two  purposes:  to  contain  the  fragments  from  the  explosion  and  to 
facilitate  the  installation  of  the  next  primer  assembly.  The  tube  around 
the  primer  assembly  contains  the  metal  and  ceramic  fragments  produced 
during  the  explosive  activation.  Clearance  with  the  outside  tube  is 
sufficient  to  allow  the  escape  of  the  explosion  gases.  The  explosion 
cavity  and  washers  also  serve  to  align  the  primer  and  ceramic  tube  so 
that  the  compressive  force  is  transmitted  directly  down  the  spool  axis. 
The  outer  casing  around  the  explosion  cavity  provides  a  guide  when  push¬ 
ing  the  assembly  to  its  closed  position. 

To  install  the  new  explosive  assembly  after  a  valve  actuation 
the  following  procedures  are  followed:  take  off  the  explosive  housing 
by  removing  the  four  bolts.  The  brass  plug,  brass  tube  holder  and  old 
explosive  should  be  removed.  Remove  the  old  ceramic  tube  fragments 
from  the  explosive  cavity.  To  make  up  the  next  explosive  assembly, 
take  the  brass  plug  and  the  ceramic  tube  and  glue  them  together  so  that 
the  ceramic  tube  is  blocked  on  one  end  and  is  centered  on  the  plug. 

Next,  remove  the  old  explosive  from  its  centering  ring  and  replace  it 
with  a  new  explosive.  Place  the  brass  tube  holder  over  the  explosive 
and  make  sure  the  explosive  (red  color)  can  be  seen  through  the  ceramic 
tube  holder.  Place  the  ceramic  tube  holder  and  explosive  into  the  ex¬ 
plosive  cavity  and  feed  the  wire  out  through  the  hole  provided  for  it. 
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The  bottom  of  the  explosive  should  touch  the  bottom  of  the  explosive 
cavity.  Next,  place  the  ceramic  tube/brass  plug  into  the  ceramic  tube 
holder.  The  brass  plug  should  sit  flush  with  the  end  of  the  explosive 
cavity. 

The  explosive  cavity  is  now  ready  for  reinstallation.  Place 
the  explosive  cavity  over  the  end  of  the  spool  shaft.  The  brass  plug 
should  be  centered  on  the  spool  shaft.  The  system  pressure  must  be 
relieved  before  reinstallation  or  the  spool  shaft  will  be  hard  to  move. 
Push  on  the  explosion  cavity  slowly  until  the  explosion  cavity  flanges 
meet  and  then  reinstall  the  four  bolts.  The  electrical  connection  must 
be  completed  before  the  valve  is  ready  to  fire. 

3.6  Testing 


The  testing  of  the  valve  was  done  to  confirm  its  operational 
characteristics.  The  test  was  performed  using  a  hydraulic  power  supply 
with  a  5  GPM  output  and  a  maximum  of  3200  psi.  After  operational  tests 
the  unit  was  tested  to  4000  psi  to  check  for  leakage. 

Testing  showed  that  there  was  no  leakage  found  with  the  power 
unit  under  a  static  3200  psi  pressure.  In  addition  the  spools  were 
shifted  manually  under  pressure  to  see  if  the  seals  would  work  dynamic¬ 
ally  and  still  no  leakage  was  found.  Even  after  explosive  actuation 
with  quick  seal  movement  there  was  no  leakage.  There  was  no  sign  of 
seal  extrusion  when  checked  after  testing. 

In  order  to  measure  the  pressure  drop  through  the  valve  block 
section  when  there  is  free  flow  (un-actuated) ,  the  pressures  above  and 
below  the  valve  were  measured.  These  pressures  were  76  and  72  psi  respec¬ 
tively.  There  is  also  a  tube  fitting  restriction  to  be  accounted  for, 
which  produces,  according  to  calculation,  over  3  psi  in  pressure  drop. 
Therefore,  at  a  5-GPM  flow  rate,  the  valve's  pressure  drop  is  less  than 
1  psi.  Because  pressure  drop  increases  by  the  square  of  the  flow,  then 
at  a  30-GPM  flow  the  pressure  drop  would  be  less  than  36  psi. 

The  operational  test  consisted  of  measuring  the  shifting  time 
of  the  valve  spools.  The  blocking  valve  was  tested  by  allowing  a  flow 
through  the  valve  and  applying  a  downstream  pressure  with  a  restric- 
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tion.  The  pressure  was  measured  using  a  pressure  transducer  located 
on  the  downstream  side  of  the  valve.  When  the  blocking  valve  was  trig¬ 
gered,  it  shifted,  shutting  off  flow  to  the  restriction.  The  loss  of 
flow  will  cause  the  pressure  to  drop  at  the  pressure  transducer.  The 
pressure  vs.  time  and  the  trigger  pulse  vs.  time  were  measured  for  sev¬ 
eral  different  pressures.  The  shift  time  is  the  difference  between 
the  trigger  pulse  and  when  the  pressure  reads  zero.  Refer  to  Figure 
7  for  the  hydraulic  schematic  of  the  test  set-up. 

The  results  of  two  trials  on  the  blocking  valve  are  shown 
in  Figure  8.  The  traces  show  that  the  valve  has  a  shifting  time  of 
less  than  12  msec.  It  is  difficult  to  determine  from  the  trigger  pulse 
trace  where  the  explosive  primer  actuates.  However,  it  seems  from  the 
trace  of  pressure  that  the  spool  itself  takes  only  about  3  msec  to  shift 
once  it  starts.  The  explosive  actuator  in  the  trial  was  actuated  by 
a  6  V.D.C.  battery.  Using  a  high  voltage  source  such  as  24  V.D.C., 
could  actuate  the  explosive  quicker  and  hence  shorten  the  total  valve 
shifting  time.  The  trigger  pulse  also  shows  voltage  "bounce"  which 
is  probably  caused  by  mechanical  vibrations  in  the  triggering  switch. 
Eliminating  this  bounce  could  shorten  the  shifting  time. 
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FIGURE  7.  HYDRAULIC  SCHEMATIC  FOR  TEST  SET-UP 
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FIGURE  8  .  OPERATIONAL  TEST  RESULTS 
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FLOW  CHART  CONVENTIONS 


The  purpose  of  this  appendix  is  to  describe  the  conventions 
used  in  the  flow  charts  (Figures  1,  2,  and  3)  in  this  report,  where 
those  conventions  differ  from  those  commonly  used  in  flow  charts. 

The  charts  consist  basically  of  vertical  sequences  of  blocks, 
with  control  passing  sequentially  down  through  the  sequences.  However, 
interspersed  in  those  sequences  are  decision  (hexagonal)  blocks,  which 
require  the  execution  of  subsequences  of  blocks  located  to  the  right 
of  the  decision  blocks.  When  the  bottom  of  a  vertical  sequence  of  blocks 
is  reached,  control  returns  to  the  decision  block  from  which  that  se¬ 
quence  began. 

As  to  the  decision  blocks  themselves,  when  the  condition  in 
the  block  is  described  by  a  FOR,  WHILE,  or  UNTIL  phrase,  the  subsequence 
to  the  right  is  executed,  respectively,  FOR  the  conditions  stated,  WHILE 
the  condition  stated  is  true,  or  UNTIL  the  condition  stated  is  true. 

If  the  decision  block  contains  an  IF  phrase,  the  subsequence  indicated 
by  the  arrow  exiting  from  the  upper  right  of  the  block  is  executed  IF 
the  condition  is  true.  If  there  is  a  subsequence  indicated  by  an  arrow 
exiting  from  the  lower  right  of  the  block,  that  subsequence  is  executed 
IF  the  condition  is  false. 

Finally,  rectangular  blocks  bounded  by  thick  lines  indicate 
sequences  described  by  other  flow  charts. 
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SCANNER  DATA  SELECTION  BOARD  LAYOUT 


The  following  page  shows  the  component  layout  of  the  scanner 
data  selection  board.  (Component  definitions  and  schematic  are  given 
in  Appendix  C. ) 
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SCANNER  DATA  SELECTION  BOARD  SCHEMATIC 


The  following  three  pages  give  a  schematic  diagram  of  the 
scanner  data  selection  board.  (The  component  layout  for  the  board 
shown  in  Appendix  B.) 
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SCANNER  DATA  CONVERSION  BOARD  PROGRAM  LISTING 


MODULE  ScannerRangeDataConversion; 

PRGGKAM  ScannerRangeDataConver sion  ilnput,  Output); 

C  0  a  S  T 

Pi  =  3. 141592654; 

Realllnteyer  =  63; 

Rea i 1  integer Pi  us  1  =  64; 

I  if  PE 

bits  =  CBitO,  Bitl,  Esit2,  6it3,  Blt4,  6it5,  Blt6,  t i 1 7  J 
Set Of  bits  s  SET  OF  bits; 

fc  y  t  e  = 

Kc,CUR  0 

CASE  integer  OF 

0;  iChrctr:  Cnar); 

1;  (BitSet:  SetOfbits) 

E  n  D ; 

TwoChar Integer  = 

A  E  C  0  R  u 

CaSc  Integer  UF 
0;  (LoCnar, 

HiCnar:  Char); 

1 :  (JntVai:  Integer) 

E !'/  D ; 

UnitVectorCompintegerindexedArray  s 

array  t-Reall lntegerPiusl . .ReaAllnteger)  OF  Integer; 

Crd  =  (x,  if,  Z); 

Trans  'atrixlnteger  = 

RECoRu 

Rotat;  ARRAY  LCrd]  at-  ARRAY  iCrd)  Oi  integer; 

Trans;  ARkAy  iCrd)  u  t  integer 

t  i-i  v  ; 

booieanRtr  = 

RcCuRt/ 

CASn,  Integer  OF 

0;  (ADSAOdr;  “Boolean); 

1;  (CtfAdgr, 

SegAdar;  integer) 

END; 

IntegerPtr  = 

KtCORt.i 

C ASl  integer  OF 

0;  (.AosAdar:  “Integer); 

1;  (CffAdor, 

SegAdar;  Integer) 

END; 

UnitVectorCorr.pintegerlndexedArrayPtr  = 


K  C  C  iJ  k  L' 

C  m  S  t  integer  Q F 

0;  (.AosAddr:  *UnitVectorCon>plnteyer  indexeoArray  j ; 

1;  tCttAcior, 

SeyAoar:  TwoCnarlnteqerj 

fcND; 

t)rasl2Conn>Bufler  a 
BtCdKu 

Scnnriodartn Trans  Matrix:  Tr an s matrix l nteger ; 
iVe^uata:  boolean 
t  tD  ; 

brasl  2Coir  mBuf  t  erPtr  =  *nr  ds  1 2CufT,tr,  butter ; 

Brdsl2Corr.':it)ijfierPtrPtr  = 

K tCuBu 

CaSl,  Integer  OF 

0;  lAosAdar:  *br ds 1 2Coir mbu t terPt r ) ; 

1 :  (C'tf  near  , 

SegAoar:  integer) 

t  n  D ; 

V  Ak 

fcrd2InCperationPtr:  BooieanPtr; 
braslxCoiruilnptnutterPtri-rorTnrai'Ptr, 
Brasl2Con-.TilQleuuf£erFtrrrotrtjra^Ptr, 
prdsl2Corr.-.iQtpt8df£erPtri'rorT.ord^Ptr, 

Krdsl2('oir.TilnptHuf  ferPtrr  roff  brdj  Ptr  , 
rirasl2Co(T.r>.loleBaf-ierPtrFrondroiptr, 

pros  1 2CorrTi  otpt  Ha  fterPtrrro  ii  br  a  lPtr:  BrdsiZCoirirfcutferPtrPtr 
nrasl2Cofr.i'TenpciofrerPtr:  brusi2Co:nnbufierPtr; 
bras  12Coitr,  iulenutter  Busy  tiomerdzPtr , 
orasi2Coirrr;ialen offer busyrro. n br<UPtr:  booieanptr; 
rortCcyte:  nyte; 

h  a  n  y  e  o  a  t  a  C  o  n  v  e  r  s  i  o  n  f  o  b  e  g  i  n  :  boolean; 
KangeoataOonversioninbrocess:  boolean; 

ScaruineftaiToer:  T*oCharinleger; 

PrevScanLinewu^oer;  Integer; 
iiextScanLineNu^oer:  T*oonarlnteyer; 

ScanPol nt guMiDer :  r*oChar integer; 
hanyei'e tuti. :  i *oChar integer ; 

Scanner  Pos  a  , 

ScannerPcs  i , 
scanner eosZ :  integer; 

ScanPtlispXrJlcnsProjFact, 
scanPtr  ispYClCosF’rouKdct  # 

ScanPtLispZOlCos  Pro-i  Fact, 
scanPtDispXFlsinProur  act , 

ScanPtPlsf.  YLlSinProdFact, 

ScanPtOispZtlSinProdFact, 

ScanPtl'ispXAzeosProuFact , 

ScanPtbispYAzcosProaract, 

S ca n P t!;  i s pZ  azCo s  Pr  ou Fac t , 

Sea  nPtl'ispXHZSinProo  Fact, 

ScanPto ispi AZbinProdFact , 

ScanPtiiSf-ZAzSlnProuFacL;  Inteoer; 

tlCosProciArrayPtr, 

fclSlnprooArrayPtr, 

AzCosi-’rodArrayPtr, 


AzSinKrodurrayPtr, 

ScanPtbiSHArreyPtr: 

UnitVectcrCoiTpLnteaertnuexedMrrdyPtr; 

ScanPtPosuatuu.BufferFullPtr, 

ScanPtPosuaturrBufferFullUnBrajHtr:  Boolean ptr; 

ScanPtPos APtr , 

ScanPtPos i Ptr , 

ScanPtPos^Ptr :  mtegerPtr; 
scanPtPosX , 

ScanPtPosi, 

ScanPtPosZ:  Integer; 

PrevPangeuataArray :  AkFA^  10, .jU  up  integer; 

ScanPoint.*umberKanqeLiataAcceptabie:  akkai  L0..31J  CF  Boolean; 
Arrayinoexl , 

Arrayinoex2;  integer; 

Array! rdex2'iv»oChar:  IwoCnarlnteger; 

Crdlnaex , 

Crd  lnaexl , 

Crdinoex2;  Crd; 


BEGIN 

Disable  interrupts; 

Bray  InCi-'eratlonPtr ,  SeqMdar  :=  8j90; 
ero2 I nG^er a t 1 onpt r .  OffAdor  ;=  0; 

Br oyinuperatlonPtr.  AosAdor*  :=  raise; 


Brdsl2Corr;i!lulehuf  ferBasyP  roinfardyptr,  SegAoar  :  =  8189; 

Br ds l /Con ii. j aietiuf t er Busy F r Oinfc r ayptr ,  OffAdor  :*  l; 

orusliConniulebutferBusyFroniBrai^tr,  s  e  g  a  a  a  r  ;s  8 1  b  y ; 
ores  lyCorru.lalebuf  f erausyFr omBraiPtr  .  GftAaar  ;=  0; 

6rasl2Co(»n. lale Buffer B a syFroir.Brayptr,  AosAadr*  ;=  False; 

Br ds  1  yConm Dtp t&uf ter PtrFron.drd2PLr •  SegAajr  :=  ei«fc; 
brdsiyConi.iC cptBuf terPtrt rombra2Ptr ,  uffAuar  :=  b; 
bra  s  1  2Con  (!:  lu  lenuf  f  er  PtrFr  of.dr  d2Prr  ,  SeyAadr  :  =  818b; 
brdsliCon  r.laieHuf f erPtrn on-drn2Pcr .  OffAaar  ;=  a; 

Bros  1 2Con  I’  lnptbuf  f  er  Ptrt  rotr.dra2Htr «  SeyAaar  ;=  >? I  « » ; 

Br  as  l  2Coit  i>  i  nptB  u  f  t  er  Pr  r  F  r  on  Bra2P  t  r ,  OifAaar  :=  o; 

Br  os  1 2Cori  ,t.C  tptBuf  f  er  Ptr  F  r  on.orulPLr ,  SeyAaar  ;=  blb7; 
Bras 1 2Corr.„LtDtbuf f erPtr FronioraiPt r ,  uttAaor  :=  o; 
brasl2Con  n  lalettuf  terPtrFron.riralPi-r,  SeyAadr  :=  bl87; 
EraslyCorri,iaieBufferPtrFron;tlralPtr,  UffAaar  ;=  h; 

brdsiyCorr.i.lnptBuf  terPtrr  romdralPur.  SeyAaar  :=  blo7; 
Brasl2Con  i,.i  nptbuf  terPcrF  ron.oraiPtr,  uffAanr  ;  =  o; 


Sea nPtPos a atom HUfter FullPtr.  Seg«aar  ;=  b  l  y  l ; 

Sea  nPtPosuatu  in  Buffer  FullPtr,  UtfMdar  ;=  0; 

ScanPtPosoatUM-ibuf f erFuiiuribrdiPtr,  SeyAddr  :  =  -6Mb; 
ScanPtFosL'atuir.butferFuiionDrdSPtr,  UffAoar  :  =  o; 

ScanPtPosxFtr.  SeyAadr  ;=  -e>14B; 

ScanPtPoSAPtr .  uftAdar  ;=  2 f 

ScanPtPostPtr.  segAodr  ;=  - 1>  1  4  S ; 

ScanPtPosiPtr,  OffAadr;*  4; 


ScanPtPosEFtr ,  segAadr  :  =  -ol45; 

ScanPtPos^Ptr.  UffAaor  :  =  b; 

Cutoy t  10CFH,  Cnr  (GBoH)); 

tlCosProdArrayPtr.  segAddr .  IntVal  ;s  2  037; 

ElCosr-rodMrrayPtr.  jffAdar.  L  o  C  n  a  r  ;  =  Cnr  l  u  o  u  H ) ; 
EiSinProoArrayPtr.  beyAaar .  int  Vai  ;=  2b49 ; 

E ioi nprcoArrayPtr .  uftAcor.  LoCnar  :=  Cnr  tOuOHj; 

AzCosPr oaAr r ayPtr  .  beyAaar,  lntval  ;=  30bi; 

AzCosProoArrayPtr .  offAaor.  LoCnar  :=  Cm  luoUH)? 

AzSinProanr rayPtr .  SegAadr,  lntval  :=  3  b  7  3 ; 

AzSin^rocArrayPtr .  oflAaar.  LoCnar  ;=  Cnr  luouRj; 

ScanPtLispArrayPtr .  SegAaar.  lntval  :=  4G&5; 

ScanPtLispArrayPtr.  OftAoar.  LoCnar  :=  Cnr  (OUOn); 

ScanLinefjyrr oer .  HiCnar  ;=  Cnr  COuOH); 

Ranoeuatun.  HiCnar  :=  C h r  (DOOri)j 

RangeoetaConvers toniohey  i  n  i-  False; 

Rangeua race over s i on i nProces s  :  =  False; 

PrevScan Line  Number  :=  -1; 

ScanFoint Hutnoer ,  intVal  :=  u; 

RLFcAX 

o«HL  fcr d21  nQperati onntr .  AbsAadr*; 

rtr iteLn  ('Roara  7  is  in  operation.'); 

Outbyt  C oi;t- M ,  Cnr  Cb)); 

which  True  oO 
BEGIN 

PrevScarii  ineNoffber  :=  ScanLineaun.oer.  lntval; 

R  K  P  L  A  i 

InByt  ( v C C h ,  PortCbyte.  Cnrcrr) 

UNTIL  (PcrtCByte.  bitSet  *  tbit il)  <>  u; 

InDyt  CoCbH,  Scanuinewuiruer ,  LoChar); 

ScanLineNuir.oer .  lntval  j=  C2b5  -  ScanLinewun-ber .  intVal)  0 J. V  « 
lnoyt  (oCAn,  RangeOatuir ,  ijoCharj; 

Ir  KangeNataConversionTohegin 

I  rlitiV 

ScanPolnt.»uit*Der.  IntVal  :=  Scanpointnumoer .  lntval  +  l; 
if  nar.aeCdtaConversionln  Process 

i  H  L 
BEGIN 


IK  bcanPointNu^oer .  lntval  =  u 


i  rl  E 

If  oCariiineMi'ncier  .  Intvai  <>  ivextScanLineivuf  ber  .  Int  val 
1.1  ► 

writebr  C '  #  ' ) ; 

if-  kCdnLinebuniber •  intvai  =  u 
1  rl  E  v 

Scar.PointMJnberKangetataAcceuLaDle  IScanfoint  u umber .  IntvalJ  :=  True 
EL  Sr 

It  ac<mf ointivurtherHangeuataAcceftaDie  IScanFcintftumber .  IntvalJ 
THE 

6canPoint  uuniner  hangefata  Accei-’ caole  [scanfointi'.uii.ber ,  mtVaU  :=  NOT 
( (Har.yeuatun,.  intvai  -  PrevKanyeuataArr ay  Ccjcanwcintf  un'ber ,  intvai) j  < 
-le ) ; 

Pr e vPanyeuata array  IScanPom LNurrber .  intvaii  :  =  FanqeLatum.  Intvai; 

If  i>car.Pointwu,T’oerf  angeoataticceptable  [bcanf  ointi'.umuer  .  Intv’alJ 
Tiff., 
b  c  G  1  f 4 

AzCosf robArrayPtr .  uffAaar.  HiCnar  :=  scanFol ntnui;>ber •  kocnar; 
AzSir.ProdArrayPtr »  ufrAaar.  hiCnar  ;=  scanPointnuir.ner .  uocnnr; 

ScanFtLispArrayPtr .  CiffAdor.  HiCnar  :  =  rtangeLatun ,  LoCnar; 

Scaur tPosA  :=  ScennerPosx  ♦  scanPtuispArrayFtr •  AosAdur*  l 
AzCosi’r oaAr ray Ptr  .  ADSAaor*  IScanPtUispX AzCospr odF  act  j  + 

AzSir.Froc  ArrayPtr .  ADsAoar*  LScanPtDispAAzSinproar  act  J  J ; 

ScanftPoSi  ;=  ScanrerPosr  ♦  Scanftuisp Array P tr •  AosAdor*  t 
AzCosPron^r rayPti .  ADsAddr*  IScanPtOispi'AzCosProdfactj  + 
AzSlrtTodMrrayPtr.  AbsAodr"  IScanft jispYAzSinf roafact J  J ; 

icarit-tPosZ  :=  ScannerPosZ  ♦  ScanPtbls^ArrayPtr .  AcsAddr"  t 
AzCosfrodAr  r  ay  Ptr .  ADsAdur**  LScanftiuspZAzCosproat  act  J  + 
AzSirProdArrayPtr.  nDsAoar*  LSeanPtbispZAzSinfrociractJ]  ; 

ah]  ut  scanPtPoSL'atuuoutf  err  uliPtr  .  AcsAudr*  Cu? 

ScanK tfosxPt r .  AbsAoar*  :=  ScanFtPosx; 
bcatii: tfos  « Ptr ,  a d s a o a r “  :=ocanPtfosi; 
bcarif  tPoszft  r  .  AtsAodr*  :=  scanPtfosZ ; 

Scauf  t  Post.  aturnUuf  ter  F  ul  lPtr ,  AnsA^or*  :=  True; 

Sca.ni- tPosbacU'iiitulf  err  uliunnruiftr  .  ADSAudr*  5=  True 

f-'L 

t  .JL 
r.  Lot 

It  rtanaer at aCo r. version  loeegin 

I  11  t  H 

Pangei.*ataConversioninProcess  :=  scanpointr.  u^ber .  irtVai.  s  15 

t.  LSt. 

Mnuevata  Conversion  lode  gin  :  = 

tscanuine  .uii'ber .  intvai  =  lbl  ( Pr evbcanLinef.upCer  =  14); 

If  fjcar.Ho intf> urr.Der .  lnt^al  =  Is 
j  n  L  ;j 
bt  Gifv 


Brds  1 /Con  mldlebuf  ter  busyFroribra2Ptr .  AbsAddr*  :  =  'irue; 

RtPt’AT 

UnllL  Ful  brds liCorr.inIalebuffe rbusyFrornbralPti  .  AfcsAadr*? 
Xr  iirusl2COir,midletju£terPtrKroinftrd2Ptr.  AbsAddr"*.  Ne'*L>ata 

1H  F.v 
BcGi  * 


Hr  ds  1  2C or*  it.  Ten  pbut  ter Ptr  :  = 

bros  1  zComir.OtptBuf  fei  PtrFroi"ord2Ptr .  AbsAddr"; 

tsrds 1 2ConiaintPtBu££erPtrFro";dr d2Ptr .  AbsMaor*  :  = 
bras  1 /  Com ,t. idle t'uf£erPcrrr6"'ero2Ptr.  AosAdar*; 


ftrasi/ComnljleButterPtrr  ro'('Dro2Ptr  .  AosAdar*  :  = 
bra&l/Cor».T.Ten'pbu£terPtr; 


brnsl/CorraTe^pBufterPtr  :  = 

Bras l  2Conii.DtPtS>ofterPtrFroi"'drdj Ptr,  AosAdar*; 

brds 3  zCon  r.  Otr tbut ter PtrFroi'.ardiPtr .  ADsAdcr*  :  = 
bras  1  2C o^n  IdlebufrerPtrFro>"brniPt.r.  AosHdor*; 


brdsl2Corr.T. TdleHutferPirFro'i’BraiPtr.  AosAdor*  :  = 
oras3  2Corr.|fTerripr!utterRtr; 

Brcs l2Cor..,„ialebu£f erPtrFro'"Drd2Ptr ,  AcsAdar**.  n  e  w  D  a  t  a 
FnO; 

Brds  1  2Conir laleButterbusyFro.ubra/Ptr ,  MesAddr*  :  =  False; 


Seen pl DispXt.lCosProd Fact  :  = 

brasl2Corrn-.rtPtBdfierHtrFronibra2Ptr.  AbSAadr**. 
Scnnr icbartn i ransnatr ix.  kotat  C X J  txj  ; 

Scan pl Lispi'tlCo&Proo tact  :  = 

L;rdsl2CoiiniUtPtBufterPtrFroif'Brd2Ptr.  AbSAcidr**. 
Scnnr  Jct-ar  tn  rransi-'atr  ix .  Rotat  t X  3  lij; 

ScanPtn ispZCICosProdFact  := 

fir  os  1  2Con ■mutptfcuf  f  er  P  t  r  t  ronibro2Ptr .  HDSAdar**, 
Scnurict-artnTrans  Matrix.  Rotat  LXj  i  Z  j ; 


Sea r.PtrispXAzslnProd Fact  ;  = 

L*r as  J  2Coit  rnUtPtbuf  f  erPtrFroividr  J2Ptr .  AbsAaar  **  . 

Scnnr ictarthi ransnatrix.  kotat  t>  J  LXJ  ; 
scanPtr lspV AzSinPr odFact  ;= 

brciil2CorrniUtptbu£ferPtrFro!iiuid2Ptr.  AasAdur**. 
Scnnr ictartnlranspatrlx,  Rotat  t Y J  CiJ; 
ScanPtbispZAzsinProaF act  := 

brds  1  2Corr.ir.ttpt  Buffer  Ptr  Frombr  d2Ptr .  AosAddr**. 
Scnnricnartn'lransnatrix.  kotat  LY3  IZJ  ; 

Sea nPtbispXr..lSinProd Fact  :  = 

bras  1  2Corr,.iUtrtbutf erPtrKroiTibrd2Ptr.  AbsAddr**. 
Sen n r V cf art n’Jrans Matrix,  kotat  LZJ  LXj; 
s  c  a  n  P  t  f  i  s  p  y  F 1 S  i  n  p  r  o  d  F  a  c  t  ;  = 

Brdsl2Com.L.tptBuffer?trFromBra2Ptr.  AbSAadr**. 
Scnnr  icc-arthiranSMatnx.  kotat  C Z  j  L  x  J  ; 
Scdi.PtcispZklBinProoH  act  :  = 


False 


6rdsl^Con.liOtPLHufterPtrt'rombrdi'Ftr.  AnSAddr*“, 
ticntirloc-artnfransMatrix.  Aotat  tZ]  iz]  ; 

ScannerPosX  := 

Brdsl2Co(Tniutpt&ufferPtrFron'brd2Ptr.  AosAddr**, 
Scnnr  icf-arthi  ransaatr  ix.  Trans  LXJ; 

Scanner fosY  :  = 

Brasl2Co(T>niOtPLBufferPtrProtT'oid2Ptr,  Aus^dar**. 
Scnnr lor.artnTransMatnx,  Trans  t Y  j ; 

ScannerFosZ  := 

t?rdsl2CorririutPtBufferPtrFron)brd2Ptr.  ApsAdcir**. 
Scnnr'i  cLartnTranSi-'atrix .  Trans  t Z 3 ; 


NextScanLine^un’ber .  Intval  ;  =  scanLineNumper .  intval  +  1; 

IF  kextscarUiine.'vurrber .  intVdJ.  =  16 

TrttH 

PextScariLineriun-.per .  Intval  ;=  0; 


LiCosFrodArray Ftr .  UffAdor. 
tlSinpr  oa«rray  Ptr  .  LifrAdar. 

ScanPtUispXAzCosProoFact  := 
EiCosProoArrayPtr.  AbsAdor* 
trSinFrod^rrayPtr .  AosAaor* 

Scan PtLUspYAzCosProo Fact  := 
E iCos Prod Array Ftr .  AosAdar* 
tlSxnFr odArrayPtr .  AosAddr* 

scanr tf- is pZazCos Proar act  :  = 
t ltosProdAr r ayPt r .  wosA-iar* 
ElSiriFrodArray Ptr .  AcsAoar* 

ScanPointi^un.her.  Intval  :  = 


nicnar  : s  PextScanLinenuirjer 
nicnar  :=  aextScanlineuurr  oer 


iScanPt  i>i  spXElCcsProdFact  J  + 
iScanPtuispXElSinProdfact) ; 


i  ocanPtOispYLlCosProdFact j  + 
iScanPtOispYtiSinProoFactj ; 


LScanPtbispZElCcsProdF act]  + 
i  ocanPtuispZElSlnFr ooFact J  ; 


L  :<  D 


Loi'nar ; 
LoCnar ; 


EnD 


APPENDIX  E 


ELEVATION  MAP  STORAGE  BOARD  PROGRAM  LISTING 


MODULE  lerrainElevat.ionuataiitoraqe^royram; 

PROGRAM  TerrainElevatlontatdStorayeProoran'  (input,  uucput); 
CONST 

I’axiiTuirHliowaDlelerrainc.levdtionLitference  =  2; 

TlPc. 

Bits  =  (eltu.  Pit],  B 1 1 2  ,  b  i  t  3  ,  b it  4  ,  bltb,  Pitb,  bit 7) 
Setofaits  =  SfcT  uf  bits; 


byte  = 

R  lCGR  u 

ChSu  Integer  u K 

o :  ( t3 i t o e t ;  SetufMits); 

1 ;  (Cnaracter  ;  Char ) 

t-D ; 

word  = 

«  c.  C  0  R  '<) 

Ca SF  integer  OF 

o;  (Lo»eruruerHyte, 

p inner  Order  byte:  SetUfnits); 
1;  (l*cdytesJ  Integer); 

2;  (Lo*erurderCnaracter , 

HignerOrnerCnaracter ;  Cnar) 

End; 

boolean pointer  = 
r<C.C  Jb'i 

Case  integer  OF 

0 :  (AusoluteAadress;  "boolean]; 
1;  (CrfsetAaoress, 

Segment Address :  integer) 

E-<D ; 

nytePointer  = 

R  r,  c  U  H  w 

C  a s  t;  integer  OF 

0;  (AbsoiuteAodress;  "byter; 

1:  (CtfsetAg dress, 

Segii'entAaaress :  integer) 

E  ■<  0 ; 

lntegerRcinter  = 

U  t.  C  U  h  v 

C  A  5  r  integer  uF 

n;  (  a  i>soi  at  e  Aod  r  es  s  ;  "inteuen? 
1;  ( C i t set Add  res s  , 

Seqn.entAdaress:  Integer) 

twL ; 

V  A  F 


Zerobyte;  byte; 
wobatuir^ord :  /-ora; 
wapx  i r.nex  , 


P  ap  i  1  noex  , 

.vapxJ  no  ex  lines  lo, 

MapYlnoexTimeslo:  Integer; 

'lerramA, 
ierraim  , 

Terrains;  integer; 
i  e  r  r  a  i  n  a  p  X  , 
lerrain^apY ;  integer; 

Ter  rainr,a^Xinaex  , 

Terrain. -lapYindexi  word; 

Terrain  us  tufr.nufterFuiiPointer, 

Lioard^TerraimjatUK.riufferFuiiPointer;  boolean  Pointer; 
TerrainAFoi nter, 

Terrain*  Pointer, 

Terrainz,Pointer:  mtegerPointer; 
TerrainArrayXposition Pointer, 

Terrain srrayYpositionPointer, 
lerrainArrayPointer:  Integer Ho inter; 


BFGig. 


boordyi  err  a  l  ni»a  t  uinBuf  t  erf  o  1 1  Po  in  Ler  #  Segment  Aadr  ess  :=  -  1638b; 
Board2JerrainL;at,irr,Buffer*,ullPoinLer.  utisetAaaress  :=  0; 


doaro^TerrainOatornBuf  fert  ullPointer ,  AusoluteAdor ess*  ;=  True; 


]  er  rainuatumbuf  f  erf  u  UPointer  ,  6egir,entAdaress  !  =  1 9 1  # 

TerrainuatumButf erFulXPointer.  Orf setAaoress  :=  o; 

i  er r a ln^a t urr.B o 1 1 er Pu  1  lPo i‘n ter  .  AusoloteAoaress*  ;=  True; 


lerrainAPolnter.  seymentxodress 
lerramAPoiiiter.  UftsetAaoress 


8191? 

2; 


ierrain/Pointer,  seyrr-entAuar  ess  :=  8l9i; 
Terrain jPointer,  uffsetA duress  :=  4 ; 


lerrainAPolnter,  oeymentAuaress 
Terrain  Z  Pol  ri  ter,  iiffsetAaoress 


8i  9 1 ; 

p; 


TerrainrtrrayXPosi tionPointer,  Seu.nentAauress 
Terrain HrrayYPOsitionPointer,  seuinentAauress 


8  19  2; 
6  2  2  4; 


Zerooyte,  Character  :=  Cnr  (OOoiii; 


Nol-atun-word ,  bignernruerCnaracter  ;=  Cnr  (Ooun); 
iNOdaturr ,.or d ,  Lo*erOrderCnaracter  :=  Cnr  (uuoni; 


tort  wap  a  Index  :=  u  TO  25  5  L*u 
b  r-u  l  im 


lerrainArrayXPositior.Pointer .  ott  set  Address  :  =  MapX  index  *  2; 
Ter rainArrayXHositionPointer .  «osoluteAodress*  :  =  NoC-atum word • 


Li.c ; 


Twony tes 


F  l-K  Kac  i  1  ndex 
BLG  1 1« 


U  TO  255  00 


ierrainArrayYPositionPointer .  of  f  set  Address  :=  K  a  p  Y I  n  d  e  x  *  2; 

Terrain 4rrayYPositionPointer.  «DSoluteA duress*  :=  RoOsturr^ord,  l*  oBytes 


fcu 


> , 


FOR  Mapi Index  :=  u  TO  2  b  b  DO 
FOk  Map x  Index  J=  u  TO  2b5  DO 
d  LG  I  N 

Terrain  Array  Pointer.  segment  Ad  dress  ;=  wapy Index  *  32  +  6  25b; 
ierrainArrayPointer .  off setAdar ess  :=  MapXInaex  *  2; 

TerramArrayPointer.  Absolute Address*  ;  =  NoUatun*ord.  T  mbytes 


fc.  y  L. ; 

lerrainpatumButterFullPoiriter .  AosoiuteAddr ess*  :=  False; 
boara2  Terra  in  uapuiTi  Buffer  i-ullPoin  ter.  AbsoiuieAddress*  ;=  False; 


aHIlf  True  uO 
bc.G  1W 


wrilLfe  mot  Ter rainuaturuBuf  f  erKui  lPoi nter  .  AosoiuteAduress*  Do; 

lerrainx  ;=  TerrainXPointer .  AosoiuteAddr  ess*; 

Terrain*  ;=  Terrain ^Pointer .  ADsoluteaaaress* ; 

Terramz  :=  TerrainZPointer,  AosoiuteAuaress*; 

TerrainDatumBufterfuliPointer.  AbsoluteAaaress*  ;  =  False; 
Hoard2TerrainuatumBuf ferFullPomter.  AosoiuteAdoress*  ;=  False; 


lerrain^apX  ;=  TerrainX 
Terrain^apY  :  =  Terrain* 

Terrain v.  a  p /  inn  ex  . 

Terrain  aapAinciex  . 

i  errainriapY  inaex  • 

i'errain'id^iinaex  . 


D 1 V  4 ; 
u  l  V  4 ; 

l*oHyte.s  ;=  TerrainMapx 
HigherurderCnaracter  :  = 
Hofyces  :=  TerrainMapT 
rtiyneruraeiCi'iaracter  :  = 


Zercay te . 


Zerony te . 


Char acter ; 


Cnaracter; 


T er r a in*r ray X fos i t ionPo in  ter  .  of tset Auoress  :  = 

Terrain-. a pXinaex.  T«*obytes»2? 

Terrain  a rrayYpositionHointer.  otfsetAadress  :  = 

Terrain -iapYindex.  Twosytes  *  2; 

IF  i  errainArrayXPositionPointer .  ADsoluteAddress*  <>  Terr  air.RapX 

i  ri  E  d 

Br.Glh 


TerramArrayPointer.  OffsetAddress  :  = 

Terrain  i a p X l n a ex.  T « o u y t e s  *  2;  • 

Map  *  l  tidex  Times  1  b  :  =  -lb; 

FuR  Map  Y  Index  :=  U  J'O  2b5  DO 
a  EG  T  n 

wap  YI  ndex  iiii.es  1  b  ;=  Map YinnexT 1 mes 1 o  +  16; 


lerrainArrayPointer .  Seqr,ientAudress  :  = 

Wdt-'i  Inaexl'imeslb  *  2  +  d2dt>; 

'lerrainArrayPointer.  AcsoluteAddress''  :=  AoDatumi*ord.  T * o n y t e s 


t.J); 

Terrain a rrayXPositlonPointer.  ADsoluteAddress*  :=  Terrain «apx 


ttili  } 

IF  ierrainArravYPositionPointer.  At-soluteAddress*  <>  T  errainhap’i 

1  rl t  ii 

dtGiN 

lerrainArrayPointer,  begnentMddress  ;= 

T e r r a i n n a p Y i n d e x .  T*onytes  *  32  ♦  h2bb; 

MdpAlndexXimeslo  :=  -it>; 

fo«  PatAlndex  :=  0  iu  25b  DO 
d  c  G 1  A 

hapAindexiimeslb  :  =  FapXin<Jexi  ifieslo  +■  ib; 

I er rainArrayPointet .  Of £sei Address  := 
tfapx Index  *  2; 

'lerrainArrayPointer.  AcsoluteAddress*  :=  Ncbatuinword,  l*obytes 
t  v ; 

I er r a i n at r ay Y pos i t i on Poin ter .  AcsoluteAddress*  :=  Terrain^apY 

tv  n  D ; 

rerrainArrayPointer  ,  SegwentAdar ess  := 

Terrain --.apYlnaex.  T*obytes  *  3/  +  y  2  5 1> ; 

’lerrainArrayPointer  .  uttsetAodress  ;  = 

Terrain xapXlnaex.  T*odytes  *  2; 

ler rainar rayPointer .  AcsoluteAnoress*  :=  lerrainZ 


APPENDIX  F 

VEHICLE  GUIDANCE  BOARD  PROGRAM  LISTING 


Ceguffr  it  =  pecoki) 

Kotat;  urin&uierAngles; 

Trans:  PtinCrd 

tMt); 

Trans  *  a  t  r  l  x  = 

k  R  C  0  K  t) 

kotat:  ARHAY  LCrdJ  OF  v'cmcrrf; 

Irans:  Ptincra 

tL  H  O  f 

Legs  =  iNone,  Ftut,  FtRt,  CrLt.  Crkt,  nrLt,  RrPiJ; 

v  end adyC  nnalype  = 

HLCUkn 

Ver.clioi- art  n  rrans  vatnx:  Trans  matrix? 

LinvexlnVehclCrdS  Vein V end Cra; 

AngvelinvenclCra:  VclnVencl Cra 

k  U ; 

venclpdyS  tt  = 

klCOIil 

Tine:  Real; 

v end i'cc.artnrrans matrix:  Trans1-  at rix 

E  is  a  ; 

Vend ‘■dy;J  ra  j  Tyne  = 

I'tCuFj 

^axirdySttidx:  Integer; 

venclodyStts :  a  i<  k  A  Y  lo. , waxpuysttsinnoylra J  J  uF  Venciadystt 
t  r.  L; ; 

sptitcType  =  ttrnsfer,  support;; 


v e  n cl l  e  g  s  1 1  = 

k  c.C  UK  u 

Srtott:  spt.stt'rype; 

Posinc-artnCrd:  Pt inter tncro 

; 

v  end  be  ^siitts  Type  =  array  ILeysj  UK  venclLegStt; 

vendue gj  rajxype  = 

MtCuku 

Lttil in e:  keal; 

Lftugt:  Real; 

Ficllf-e:  Real; 

Pict-osInvendCrri.  PtlnVenclCrd; 

CttiJn.e;  neal; 

Cttnqt«in:  neal; 

C 1 1  1  a  t  n  a  x  :  «  e  a  1 ; 

i\x  tt  hui  ntar  thcro  :  PtlnfcartnCra 

E  -li  C  j 

i/enciLejirajskec  = 

H  t-C  U  Ft 

rtdxnejjrajlax;  integer; 

Vehclue glrajs  :  aPkay  to. . daxneg fra^ s InLeg I ro jsRecJ  uF  vehclneuTrajiype 

t,L; 

vencluegs i ra jsT ype  =  AkkA i  [LeosJ  Ur  VenclLeQlrajsRec; 


MODul t  Veniclelra jectoryRianning; 

PRGGRAM  vehiclelraiectoryPlannina  i  Input,  Output) 

C  u  iv  5  ri 

Pi  =  J. l4l592oS4? 

Kaxbdy  St  t  s  I  nBay  i  ra  j  =  bo? 
MdXLeglrajslnLegTrajsKec  =  iu; 

vehclCtrlovenclTop  =  0  •  u  ? 

venclCtrlovenclbtuegs  =  l.t>25; 

venciOtrToVer.clntLeqs  =  -  1,023; 
VenciCtr’ioVehclrtbegs  =  5 ,  u ; 

VenclCtri oVehclCroegs  =  O.o; 

VenclCt rTovenclKrLegs  =  -  5.u; 
VenclCtrToVehclCG  =  -  2.333333? 

VenclAltituje  =  6.0; 

vencl,ax'lransvcl  =  d.O? 

Vencl'-axTransAcc  =  4.0; 
vend  axPotatvel  =  o.5; 
v er.cl  •  axwotatrtcc  =  0.25; 

Venci.inTurngKaa  =  lb.o? 

4  ax  uegR  t  r ,  i  i.o  =  u.d; 

LegRtnlirr  =  0  ,  b  ; 

LeguftTirr  =  u ,  7. ; 

Legb'lcTirr  =  0.2; 

GuiaAlgi tunbxecintrvl  =  0.25u0u; 

T  i  Pt 

J*oCh3rInteger  = 

h tCuRw 

C  Ai>,t.  integer  uF 
0;  (.LoCnar, 

MiCnar;  Cnar); 

1;  cltgrVal;  integer) 

K  m  C ; 

Cxd  -  ( a ,  1  f  Z  ) ; 

PtlnCro  =  ARRAY  [Cr j)  OF  Real; 
v’cinCrd  =  A  a  R  a  Y  LCrd)  OF  Real; 

tartnCru  =  (Farthx,  fcarthY,  tarthZ); 

Ftlntar thCrd  =  array  itartnCroj  OF  Real; 
vclnEartnCrd  =  ARRAY  IFartnCroj  OF  Reai; 

(/enclCru  =  (Vehclx,  vencl'Y,  VenclZ); 
PtlnVehclCro  =  ARRAY  IVenclCraj  OF  Real; 
vcInVendCru  =  array  [VenclCrai  OF  Real; 

tulerAngles  =  da*,  Fch,  Rll); 

ur lot uler Angles  =  array  [Euler Angles J  OF  Real 


Vendue  jOnd  = 

K  tCuRt.' 

Spt^tt:  sptsttlype; 

VehdueyCftnalraj:  VehduegTrajiype 

t  D  ; 

venduegsCrrnasType  =  AftXAY  tLeus]  Or  venclLegCimd; 

boolean Ptr  = 
ft  ECU K  L> 

C A S t  integer  JR 

0;  CAosAddr:  “boolean); 
l;  tdfAddr, 

SeaAaar:  Integer) 

E  a  U ; 

Integer  Pointer  = 

ft  E  C  o  ft  L' 

CaSc,  Integer  uF 

0:  CAosAdar;  “Integer); 
l  ;  i  fi  t  f  Adar  , 

S  e  g  A  a  g  r ;  integer) 

t  iv  u ; 

BrosMCoirMiButfei  = 
n  L  C  U  ft  u 

CormtTiTe:  Real; 

Vencliotartn Trans Metrix:  Txans Matrix; 
VePclbinvelir,  veiiciCro, 
vencIAngvelin^enclCra;  Vein VenclCra » 
v endue  gsatts:  venclLegsStts • ype; 

Fr*aVelHqst, 

dideVelHqst, 

I  urn  Velnqst;  Keel; 

Vendue g st>iids»  VencluegsCm noslype; 

^e*L'Sta;  boolean 
F  n  u ; 

erdsl4ConinBufterPtr  = 
nECuftu 

CASE  Integer  ijF 

u j  t a l> s a d u r :  *t)rdsl4Co ft mt» utter); 

J :  if  t f Ador , 

SegMdor;  Integer) 

Ei.u; 

braslACof^bufferPtrPtr  = 
r<  E  C  U  ft  U 

C«St  Integer  U F 

u ;  (.ADSAdjr;  “braslACoft^burferPtr); 

) :  (CffAddr, 

SeaAdar;  integer) 

E  M) ; 

ProsAiCoiti"  buffer  = 

ft  L  C  J  ft  L’ 

Vend  iid  y  T  raj :  vehd'bciylraj'iyue; 
v ebclue gsl rajs:  Vehcltegs'i'ra  tsType; 
ne*oata;  boolean 
f  -i  U ; 


drds4iCon'n.BofterPtr  = 

Hfc,COk  J 

CA6t  Integer  Uf 

0:  (AbSMOdr:  *Brds41CorrmHurter ) ; 
1 :  ^CtfAdor , 

SeqAdbr:  integer) 

£nd; 


drasAlCoiriiitjuffeiPtrPtr  = 

RECORD 

CASc.  integer  Of" 

0:  CAoskadr;  *Brds41Coir;ndurterPtr); 

1 :  CGttAddr, 

SegAddr:  integer) 

E  :<  L ; 


v  A  A 


Bragin Operation Ptr# 

oral  in Operation Ptr :  BooxearPtr; 

Bras  1 4Con  ininptt'ai  ter Ptri*  rof?  bra  i  pt r  , 
bras  1  4Con  blalebuf  terPtrr'roirbrd  j  P t r , 

Bros  1  gCoirinCtPt nut terPtr r  rorraruiptr, 
draslAConrnilnptbutierPtrrroffora^Ptr, 
brdslACorr.nidieBuf£'3rPtrrroni.bro**Ptr, 

erasi4Coir.r.0tptelafferPtrrrofTDra«*?tr:  Brdsl4Corr.riButterRtrPtr 

brosJACon.irJempbofferPtr:  »rasl<* Co intr butter Ptr; 

braslACon^idlebuffereusyErcinBrblFtr, 

trasl  4CoirnI  ole  nut  ter  busy  f  ro<rmro4Ptr ;  booieanPtr; 

bras4  lCoir^inptbgt  rerPtrt*  roit.ora  APtr  f 

brosiiCoffiiidlenufterPtrrroiT.draiPtr, 

Bros  a  j  Cor.  iv'Otntbuf  f  erPt  rf  rorrorajptr, 
brasAlConiiinptbutterPtrfronBra^Ftr, 
rirasAiCofrn.lalebufferFtrf'ronbra^Ptr, 

Bros  4  ic  off  li.Otptbuf  ierPtrF  roff.bro**Ptr  :  bras4lcorc'r  But  ter  Ptr  Ptr 
DrosAlCoir.i'ieropButferPtr:  6ras4iCotTirrbu££erPtr; 
bros^irornrlaleAutterBusyfrO'iinra].  Ptr, 

Pras4i.CorrrriuleB  ufferbusyFron.br  n^ptr;  boolean  Ptr; 

1 r  r DA r  r  aFos Ptr  , 
rrrnArr tPosPtr , 

Irrr.ArrAFgsseiowPtr , 

IrrnArrxPosAbovePtr, 

TrrnArrxFostjPio*Ptr, 
irrnArrYFosAbovePtr;  tntegerRo  inter; 
fJoDatltgr:  l*oCnarinteaer; 

KoDatKeal;  Heal; 

Plus  I  minty:  Peal; 

VendoGirVenciCfa:  Ptlnvenclcra; 

LegnasePos I nVenclCr j :  An nh\  UegsJ  OF  PtlnvehclCrd; 

Trar.s  veiPqst'-iaq:  Real; 

Transficckgstwaq,  KOtatAccRqstwag:  Real; 
velneoFctr,  Accneufctr:  Real; 
vend odyTra j :  vehclBdyTrajlype? 
vehclbegsirajs:  vehclLegsirajs'j'ype; 

Leg:  Leys; 


FjivCTiOs  irrnblev  ( 
I  r  r  n  X , 

irrnt:  Real)  : 


Real; 


VAR 

TrrnMarx, 

Irrntfapl:  I n t e o e r ; 
lrr n.^apAldx , 

Ir  r nv  ap  i l ox  :  rwoCnar Integer ; 
TrrnArrFtr:  IntegerPointer ; 
Trrnk. levltur:  Integer; 


BEGIN 


Irrn"atX  ;=  Round  (Trrn*  *  b.yj  i 1 v  4; 

'/  r  r  r.  ►.  a  p  y  :  =  Round  ( T  r  r  r.  Y  *  0 . 0  1  u  1  v  4 ; 

Irrnf-  anAldx .  ltgrval  :=  TrrnHauA? 
i  rrn*- at'A  lux .  hicnar  :=  cnr  (oowhj; 


ri  r  rmv af  y  1  ax .  ltgrval  :=  X  r  r  n  (•!  a  v  i ; 
i  rrr.’iac  it  lux .  rtlcnar  ;=  cnr  tOuori); 


YrrnAr  rAf-osRtr .  otfAaar  :=  rirn^ap>lax.  ltgrval  *  2; 
TrrnArr Y RosRtr •  CtiAoar  ;=  TrrnnapYiax.  ltgrval  *  2; 


u 

CTrrnArrARosPtr .  AbsAddr*  <>  rrrriMapx)  L>h 
(TrrnArrYPosPtr .  AtsAddr*  <>  IrrnRapY) 

J  ri  b  f, 

irrnfciev  :  =  Nci-atReel 

Lb£>  r 
HLVlf' 


TrrnArrRtr.  «s e g A «J a r  :  =  TrrnMapYldx,  ltgrval  *  3  2  +  <3  2  d  0 
Irrr.ArrRtr.  OttAddr  :=  Trrniv  apxlcx,  ltgrval  *  2? 

'iirr.Fievityr  :  =  TrrnMrrRtr.  MDSAdar*; 

It-  i  rinulevitgr  =  Noudtityr.  ltgrval 

Inf 

Tr  r  ,u  iev  ;=  voDatkeai 

tE.Su 

Inn  Kiev  :  =  VrrnElevlLgr  /  B.u 


t  ’  v  D 


t.-'u; 


f  u  A  C  1 1 C  >  i  r  r  n  F  t  h  d  ( 
'1  l  r  n  a  , 

IrrnY:  heal): 

Real; 

VAR 

1 r rub ap a , 
Trrnfoap* , 

1 1  rnRanXBelo* , 
TrrnNapAADOve, 


Irrni^dp  VBelov* , 
lrrnAdpYAoove;  Inteyer; 

Trrn^apxiax, 

1  r  r  n  *  a  p  Y I  d  x  , 

TrrnNarXldxbeiOtt , 
lrrtiAapXlaxAoove, 

I rrnfcar i Idxaeio*  , 

Irrn^dpYIaxAnovet  T-voCharlnteger; 

TrrnArrFtr:  IntegerPointer; 

1  r  rr.K  levl  tar  , 

AdjJrrnblevitgr:  Inteyer; 

bLGilv 

IrrnPapx  :  =  Hound  CTrrnX  *  w .  u  »  Div  4 ; 
frrrPacY  :  =  Hound  CTrrnY  *  6,0)  DIV  4; 

i rrnf apxPe 1 o*  :  =  TrrnKapx  -  i; 
i rr n«dPAAoove  :=  TrrnbapX  +  l; 
f r rnfodpY be lo*  ;=  irrnf'.apY  -  1; 

TrrnMapYAoove  :=  i'rrnMapY  +  l; 

irrrP-aCAJ dx .  ltyrval  :  =  Trrnnaox; 

‘i rrr.fr-apx-Iux.  rliChar  ;=  Cnr  10  1? 

■frrn^apilax*  1 t g r  v a l  :  =  1  r r n * a p Y  ; 

Irrn^dPilax,  HiCnar  :=  Cnr  (. vj; 

'irrnf' apx laxMeiow .  Itgrval  ;=  Trrni'iewXtfeiow ; 

'£  r  r  n  p  a  p  a  I  d  x  b  e  i  o  *  •  H  i  C  n  a  r  :  =  C  n  r  CO); 

Trrnv dPxldxAcove.  ltgrvdi  ;=  irri)MpA»uove; 
l'rrn^dPAl axAoove.  HiCnar  :=  Cnr  CO); 
irrn'vdpi  iuxoelo*  .  Itgrval  :  =  I  r  r  n  m  a  p  Y  o  e  1 0  <» ; 

Irm^afYIcxaelo*,  HiCnar  :=  Cnr  U<  J ; 

Irrn/at  y ldXMnove .  Itgrval  ;  =  ]r rn'iapY  Above; 

TrrnVarYIaxncove.  HiCnar  ;=  Cnr  (0); 

IrrnAxrAPosPtr.  UffAaar  :=  irrn^apx lax .  itgrval  *  2; 

Xr  rn  Ar  r  x  PosPtr  .  OffAaar  :=  Tr r n XapY lux ,  itgrval  *  2; 

TrrnArrXPosueio-Ptr.  ut-raadr  ;■=  irrnMdpxiaxbeioi*.  ltqrVdl  *  2 

IrmArr/^osAnovePtr.  uftAaor  ;=  TrrnMdpAidxAccve.  1 1 g r v a 1  *  2 

1 r rnArr j PosoexOAPtr .  if iAuor  ;=  1 1 rnaapY iuxdeio *  ,  itgrval  *  2 

XrrnArrY^osArovePtr.  uftAnor  :=  TrrnMapiloxAbove.  i  t  q  r  v  a  1  *  2 

IF 

( J rrnArrXPosPtr  ,  AhsAadr*  <>  irrnKapX)  OH 
(  rr rnArr YPosPtr .  AbsAuor*  <>  irrnnapYl  oh 
(irrrArrXPosPelowPtr  •  AosAodr*  <>  Tr rnAapXBelc*  )  oh 

(irrnArrXPosAuoveptr,  AosAbur*  <>  'Jrrnik>apXauovej  o n 

(i'rrnArrYPosPelo^ptr.  AnsAcor*  <>  TrrnMapit»elo«)  uH 

Cirrr>ArrYPosADOveHtr.  AosAour*  <>  Irrn^apYADovej 

Tab  '* 

IrrnFtha  ;=  rgoHatHeal 

t  LS  n 

titGlt'' 

IrrnArrPtr.  SegAdJr  :=  Trrn3apYIdx.  Itgrval  *  32  +  *2bo; 
irrnArrPtr.  OffAdur  ;=  rrrn^.dpXiox .  itgrval  *  2; 


IrrntievItQr 


IrrnArrPtr.  mosAout*; 


li-  irrntlev.Lt.gr  =  Nouatltgr.  itgrvai 

IMfc  . 

'irtuFtno  ;=  fooUatHeal 
tbS^. 

£j  t  G  1  f'l 

IrrnArrPtr.  SegAdor  :=  Trrni'iaptJux  ,  itgrvai  *  j2  +  825o; 

TrrnArrPtr.  utfAoor  :=  TrrnAapX  idxbelo* ,  xtgrvai  *  2 ; 

Avi  1  j  r  rntlevitgr  :=  J'rrnArrttr.  MDSAadr*; 

IF  Ad  j  J'rrntievl  tgr  =  NcbatJtgr,  Itgrvai 

1  .if  :v 

irrnf ina  : =  No Bat Beal 
t  bS  t, 

Jr  Abb  LTrrntlevltgr  -  Ad  j  irrntlevl  tyr )  >  3 

1  it  :» 

Trrnftnd  ;=  NoPatHeal 
fc  LS c. 

BcGir. 

TrrnArrPtr.  SegAadr  :=  Trr ndapX lax  .  Itgrvai  *  32  +  bxbti; 

irrnArrPtr.  Of  t  Aaar  :=  irrrMapAiaxADove,  Itgrvai  *  2; 

Ajjirrntievitgr  :=  TrrnArrPtr.  AosAdar*; 

JF  Md  y\  rrntlevltgr  =  Nouatltgr .  itgrvai 

1  A  F  ,v‘ 

TrrnFthd  :=  NoDatneal 

t  Li  S  tU 

If  Ans  iTrrnhlevItgr  -  A<ijTrrnti.evitgr)  >  3 

IMF', 

TrrnFtnj  ;=  NoDatneal 

tLSL 
B  h,  G  1  N 


lirnArrPti.  SeuAdor  :=  LrrnMapiiax&elo*.  itgrvai  *  3  2  +  B25o; 
irrnArrttr.  CttAddr  :=  TrrnMapAlax  .  Itgrvai  *  2; 

Agj Trrntlevltgr  :=  TrrnArrPtr.  AcsAddr*; 

J  r  Ac  jT  rrntlevltgr  =  i.uuatltgr.  ltqrVal 

T  rtf  IV 

TrrnFt hq  :  =  rvobatReal 
f  lS^ 

Ir  aps  (Trrntlevltgr  -  AajT rrntlevltgr)  >  3 

1  1 1 1  n 

Trrnftna:=NobatKeal 
£  b  S  t 
BtG  iw 

TrrnArrPtr.  begAadr  s=  Trrnv>ap¥  laxAcove .  iturval  *  32  +  82bb 
TrrnArrPtr,  UffAdar  :=  T  r  r  n  "i  a  p  X  I  a  x  ,  Itgrvai  *  2; 

Ad1irrn-_levitgr  :=  TrrnArrPtr.  Abs  Aod  r “ » 

If  Ad j Tr rnt ie v 1 tgr  =  NoUatltyr.  Itgrvai 
TriEi, 

TrrnFtna  :=  toDatReei 

Eb.Sr, 

It  A  i>  s  (TrrnFlevltqr  -  Ad  j  Trrntlevltgr )  >  i 


oouatkedi 


THEn 

TrrnF'tno  :  = 

ELSE. 

TrrnFtno  :=  Trrntlevjtyr  /  b.o 

t  h  D 
E  w  L 
fc  "iP 
E„r 
END 
E  i « L’ ; 


FuwCliE'n  arc  (an/:  ( 

Xvaiue, 

lvalue:  Realj; 

Real? 

uLGH. 

IE  AValue  >  0.0 

IrtE.v 

Arcian2  :=  ArcTan  (j Value  /  x value) 
t  L5  t, 

IF  xvalue  <  o.o 
i  HE  w 

Mrclan2  :=  ArcTan  (YValue  /  xvaiue)  +  Hi 
ELSE 

IF  Y Value  >  0.0 

lrttr. 

Arcian2  :=  Pi  /  2.0 
t  b  S  E 

Arciao2  :=  -  Pi  /  2.0 
E  0  D ; 


PkQCEuUkE  CalcTr ans^atr  ixF  ron- Post'eguf  f  I 

Var  Trensvatrixvar :  Trans -latrix; 
ueguf  f  raft  v  ar ;  ueoof  E  r-iir ) ; 

VAR 

Cosia*,  Siniaw  , 

Cos^cn,  Sinpcn, 

Cos k 31,  5 i n k  1 1 :  Keai; 

BEGIN 

Cos  i  a*  :=  Cos  (DegOtFrcnr.var.  Rutat  lia*j); 

Sinya*  :=  Sin  (DeyUfFra.i-var.  Kocai  [Ydwj); 

CosPcn  :=  Cos  (ueyQfF  ra»>Var  •  Kotat  tPcnj); 

SinPcn  Sin  (DegOfFroiivar.  Rotat  [Pcnj); 

CosRli  :=  Cos  (uegDfFrdnVar.  Kotat  tKlij); 

SiMRli  :=  Sin  (oegOtErGr:  var.  Kotat  t«uj  ); 

iransiatrixvar.  notat  ixj  ixj  :=  Cosh*  *  cosFcn 

Trans  .atr lx Var .  Rotat  [xJ  IYJ  :=  SinYa*  *  cosecn 

i  r  a  n  s a  t  r  i  x  v  a  r  .  Rotat  [X3  [  Z  J  :=  -  Sin^cn 


Trans "atrixvar.  Rotat  [iJ  C X j 


C  o  s  x  a  *  *  S  i  n  P  c  n 
Iransnatr ix  Var . 
Sinva*  *  SinPch 
I rans.iatrix  var . 
i*  *)  CosPch 


*  SinKll  -  Sima* 
Kotat  m  l  Y  J  s  = 

*  SinKll  t  Costa* 
Kotat  C i 3  LZi  := 

*  SinKll; 


*  CoskII; 

*  CoskII ; 


Trans  ia  trix  var  . 
CosVav  *  SinPch 
irans'-iatrixvar. 
Sin  raw  *  sin Pen 
Trans  Matrix  var  • 
(♦  *)  CosPch 


Kotat  t Z J  1X3  ;  = 

*  CoskIt  +  Sima*  *  SinKll; 

Kotat  [ZJ  l V  3  :  = 

*  CoskII  -  Cos i a*  *  SinKll; 
Kotat  iZ)  C  Z  J  :  = 

*  Coskll; 


1 r ans 'at r ix var  .  Trans  LXJ 
Trans,  atnx  var  .  Trans  til 
Trans' atnxVar.  Trans  LZj 


DeyCjf Fratf'Var .  Trans  C X  J ; 
uegOf Fra*’ Var ,  Trans  IV); 
OeqGt Frarr-Var .  Irons  LZj 


t ,  j  L) ; 


P k i‘ c fc. l> u k K  Ca lcPos Deg Of  FrdnT-  rorii Trans wa tr ix  ( 
v  a  K  Deg  jf  Fran  var:  Deg  Jit  ran. ; 

Trans. vaLrixVar:  Trans Matrix); 

btol.'v 


D  e  g  o  f  Fran  var.  Kotat  [  v  a  *  j  :=  at  clan 2  C 

I  r  ans.'ia  tr  ix  var  .  Kotat  LXJ  LaJ,  Tiansnatrixvar.  notat  IX  J  IV)); 

Deguit  ran  var .  Kotat  IPcn)  :  =  Arclar2  ( 

SgK  t  ( 

Sqr  Cirars Matrix Var.  Kotat  IV J  IZJ)  +  snr  (TrarsMdtrixvdr,  Rotat  LZJ 
-  l ransnatr ix Var .  Rotat  tx)  IZj); 


begot r ran var  •  Fotat  lull)  :=  Ardan2  C 

Irat.s  matrix  var.  Rotat  LZJ  IZJ,  Trans^atrixvar.  Kctat  Lv]  IZj); 


DegufFronVar.  Trans  [  a  j 
b  e  y  o  f  t  r  a  ii  V  a  r  .  Trans  t  x  3 
Degutc  ra<r  Var .  Trans  izj 

hniJ ; 


TransMatr ix var  .  Trans  IX) ; 
Trans v atnxvar.  Trans  t V J ; 
Trans  Matrix  Var.  Trans  [Zj 


Pr.GCtuUrcF  T r ns f  r n  Pt  loFa r  tnCr dFr  v enclCr d  t 
v a k  Pt) nKar tnCrdVar :  Ptinc.artnCra; 

Vend io^arth Iran s Matrix:  1  tans Matrix; 
PtlnVenclCruVar:  FtinvenclCroj; 

clGIM 


Ptini- ar  tntraVdr  LtartnX) 
vehcliobartnTr  an  s Matrix. 
vehclioc,artnTrans  matrix. 
Vend iOtartn Trans matrix. 
vehclTonartn'Iia  ns  Matrix. 

PtinFar thCroVar  ItartnVJ 
vehcl j o Fart hi rans Matrix, 
venclio Fa rtnTrans Matrix, 
vend Jonartn Trans  Matrix. 


Rotat 

LXJ 

LXJ 

* 

PtlnvenciCrdVar 

l  veneix) 

Rotat 

IV  J 

LXJ 

PtlnvenciCrdVar 

LVencl v  J 

Kotat 

iZJ 

ix  i 

* 

PtinvenciCrn var 

L  V  e  n  cl l j 

+ 

r  r  a  n  s 

LXJ ; 

•  "* 

Kotat 

tx) 

IV  J 

♦ 

PtlnvehciCrdVar 

L VehclxJ 

♦ 

Kotat 

iv  j 

LV  J 

* 

Ptln venciCro  var 

L VenclVJ 

Kotat 

LZJ 

i  i  J 

* 

PtlnvenciCrdVar 

L  Vend  x) 

IZJ  J  J 


vencl'iOcJartnTranstfatrix. 

Ptlnfc  ar  tnCroVar  [EarthZJ 
vencliotartriTrans'iatriXi 
VehcliOEartnTra ns Matrix. 
Vehciioc-artnTrans.’iatrix, 
VendJOu,artnTrans  Matrix, 


lrans 

m ; 

•  — 

•  ” 

Hotat 

[XJ 

IZJ 

¥ 

PtlnvenciCrcivar 

[ venclxJ 

Kotat 

[  Y  J 

[ZJ 

¥ 

Ptlr.venclCrcvar 

[Vend  i  J 

+ 

Hotat 

Trans 

[ZJ 

[ZJ 

tZJ 

¥ 

PtinvendCrovar 

i vencizj 

+ 

fc.  h  u ; 


PKGCtDUKf,  T  rnsf  riTivcloEar  tnCrdfc  r  vehdCrd  [ 
vak  VcInEarthCruVar :  Vein cart ncra; 
VeticiioGaitnJransMatrix:  Iran s Matrix; 
VcinVenclCroVar:  vcinvehciCra); 

otGIW 


VcInEar thCraVar  ttartnXJ 
v en c  1 1  ota r t n’J  r an s Ma t r  i x , 
VencliohlartnTrans^atrix . 
V ehcl i ouartnlrans matrix. 

vdntartrCraVar  luartnij 
Vehciioc-artnirans  matrix, 
Venciiot-artn ’lrans  Matrix, 
vend iodartn Trans matrix. 

VcinLarthCraVar  [f.artnZJ 
vend  iohartnl  rans Matrix . 
VehciiOtartnTransMatrix. 
Vend  roc.artnTra  ns  (Matrix  . 


Kotat 

tx] 

[XJ 

♦ 

v  cl  n  v  e  h  c  i  C  r  cn  a  r 

IVehclAJ  + 

Kota  c 

[Y  J 

[XJ 

¥ 

VcinvenciCrdVar 

[ v  e  n  cl Y  J  + 

Kotat 

[ZJ 

[XJ 

4 

VcinvenciCrdVar 

[VenclZJ ; 

*  — ■ 

*  "* 

Kotat 

[XJ 

[Y  J 

¥ 

Vclnvencitravar 

[VenclxJ  t 

Kotat 

IY  J 

[Y  J 

¥ 

VcinvenciCrdVar 

[  v  e  n  d  Y  J  + 

Kotat 

[ZJ 

[Y  J 

¥ 

VcInvendCrdvar 

[ VenclZJ ; 

•  — 

• 

KO  t  <a  t 

[XJ 

[ZJ 

¥ 

VcinVenclCroVar 

[VenclXJ  + 

KOtdt 

[YJ 

[ZJ 

¥ 

VcInvenciCruvar 

[Vend  i  J  + 

ho  tat 

[ZJ 

[ZJ 

¥ 

vcinvenciCravar 

[VenclZJ ; 

fc  i<  u ; 


PKUCfc. L)UkF  7  r ns  t  r t"Pt  i  o  VenciCrdF  riiar thCrd  C 
vAK  FtlnvehclCrdVar:  PtinVenclCro; 

Vend  io  r.  art  aTrans  Matrix:  lrans  Matrix; 
PtinEar tncra var  :  Pt InEar tnCra j ; 

Vhk 

In  ppt  Jlrt.artnCrcivar :  PtinfcartnCra; 


TmpFtintartnCrd var  [LartnxJ  := 

Ptlntar tnCrd Var  f  tartnxJ  -  vend']  ofc:ar  tnTranstfatr  ix .  Trans  [XJ; 
i mppt J ndartnCrd var  ItartnYj  := 

Ptlnfc arthCrd var  [tartnYJ  -  Vendlot'artnrransMatrix .  Trans  [YJ; 
TmpPtincartnCravar  LfcartnZJ  := 

PtlnEdr thCraVar  [tartnZJ  -  VendloEartnTransMatrix .  Trans  [ZJ; 


PllnVendCraVar  [venclxj 
vend  ioc.artn  Trans  Matrix. 
VenclJoEartnTrans.Matrix. 
vend  i  ocartnT  ran  s  matrix. 

PtinV ehclCraVar  [venclYJ 
Vend lCcartnTrans natrix. 


Kotat 

LX  J 

[XJ 

¥ 

TtrpPtlnfcartncraVar 

ItarthXJ  + 

KOtdt 

[XJ 

[YJ 

¥ 

Tn.pPtlnrartnCrdVar 

t  £  a  r  t  n  Y  J  t 

Kotat 

[XJ 

[ZJ 

¥ 

TTiipPtlnEartncrdVar 

[ tartnZ j ; 

»  — 

• 

Kotat 

[YJ 

[XJ 

¥ 

i  n  p  P 1 1  n  fc.  a  r  t  n  v.-  r  o  V  a  r 

itartnAJ  + 

»/ehcl  l  otldrt n Trans '<i3tr lx • 
Ve n cl lotartn Trans matrix. 

PClnVenclCraVar  l  vend  Z] 
v  e  n  c  1 1  o  c  a  r  t  n  T  r  a  fi  s  s  a  t  r  1  x . 
vend  I otartn'ira  ns  matrix, 
vend  x  o  e,  a  r  t  ii  Trans  matrix. 


Rotat 

Ll  J 

[1J 

* 

TmpP.tlntartnCrdVar 

[tartnY]  + 

Rotat 

m 

IZJ 

* 

la.  pFtln  tartnCr  dVar 

[r.artnZ]  ; 

•  «— 

•  • 

Rotat 

LZJ 

tx] 

* 

impPtlnfcartnOraVar 

[tartnX]  + 

Rotat 

IZ  J 

id 

TmpPtlnr.artncraVar 

1 1  a  r  t  n  Y  ]  t 

Rotat 

[  Z] 

tZJ 

* 

ImpPtintartnCraVar 

ttartnZ] 

1*  u.vC'X  10  i  t  tinbimi  ts  ( 

Leg :  be  os ; 

f  tPosir.LsrtnCrci:  Ft.  in  tar  tnCra; 

ven  el  Tctartn  Trans  i-atrix:  iransMatrix): 

t-  o  clear; 


C  J  im  b  ] 


i>,ax  L,egl.  ng  t  n 
winue  ilngth 
A'  l  n  «  c  o  A  n  g  1  e 
«  a  >  a  c  a  a  n  g  l  e 
*  a  L  s  p 1 c 
K  l  r  r  w  a  C  S  p  1 C 


V  h  H 


7.dJ3Jj; 
3 .  *  i  3  3  3  i ; 
o ,  u  b  b ; 
0.430? 

3 .  o ; 

3«o; 


FtFoslnvenciCrd;  PtlnVenclCrci; 
F  t  f  *  d  o  s  p  1  c , 

FtAcoMPgle , 

►  tlegury tn :  Peal? 

F 1 1  n  F  *  d  i>  s  p  1  c  L 1  m  i  t  s , 

F 1 1  nAoc  any  lei  i  it.  x  t  s , 

F cl nleqongtn Limits:  boolean; 


L  t(j  I  .v 


I  rnstr  n  r't  iovenciCr  dFrl.ar  thCro  » 

f  t?os  ]  r,  vender  vend  lotartn  Trans  matrix,  FtPcsintartncrd); 

r  t  F  *  a  -  s  d  c  :  = 

F  t  Pos  r  r  v  e  nc  1C  r  <1  ivenciXJ  -  LegoaseposlnvehclCrd  ItegJ  Iv'enclXJ; 
rClr.Fvwogst-lC  Limits  :  = 

(FtF*gLsdc  >  =  MinFwdL’splc)  a.io  (  f  tF^dosplc  <  =  y.axF  wdL/splc) ; 

IF  uoc  CCrd  (Leg)) 
lit  ,\ 

F  t  Aoo  Ang  1  e  :  =  ArcTan2  ( 

-  F cFos Ln venciCrd  tvenclZJ, 

(FtPosJnvenclCrd  IVencld  -  LeupasePoslnVenclcra  lleyj  IVenclYJ}) 

£  L  5  t 

r  t A o d Angle  :=  Arcian2  ( 

-  FiFoslnVenciCrd  iVehclZ], 

-  (p  tr’cslnvehclCra  ivencld  -  i.egoaseRoslnVehclCrd  l  Led  IVehcliJ))? 
FtlnAc'dApgleLinits  :  = 

C F  t Ate. Ai.q le  >=  i^inAbdMngle)  Am>  iFtAodHngle  <=  RaxApdArgle) ; 


r tLeguny t  n 


•  ■— 


SqPt  l 


Sqr  tr  tFoslnvenclCrd  [vehclYJ  -  LeunaseRos  inV  ehclC'rd  [i.eg3  IVenclYJ)  + 
Sqr  ( f  tfosInVenclCr  J  [VenclZJ  -  LeooasePosinVenclCra  [begj  [  VenclZ] 3 ) ; 

FtlnlegLngtnLimits  :  = 

(FtLegLngth  >=  FinLegLngtn)  Ai.u  (FtLegLny th  <=  NaxLeoLny tn) ; 

FtlnLinits  := 

F  tlnF *dusplcLin'i ts  Ani.  f  tinADaMnQieLirrits  AND  F  t  InLegLngthLi irits 


LnD  ; 


FiMiCT  lG-«  Calc vehclHuyTr dj  l 
rr*dACCnust, 

^IaeAccKqst , 
lurnAccnast:  Real; 

I'urrr.tl  ire :  Real; 

ve h cl locaitmrans matrix:  1  fans Matrix? 
^ehclLinvellnvenciCrd, 
vchclAngvelinVenciCro:  v  c  I  n  v  e  n  c  l  c  r  c ; 
v.hH  veNcJ.Doy  fraj  ;  Vehclaayl  ra  j  j’yt>e)  i 
Boolean;  • 

v  A  H 

v  e  D  c  l  p  c  s  i  j  e  a  j  f-  f>  r  d  n  -v  p  i  t  a  r  t  n  C  r  d :  L  ecu  f  F  r  d  tr. ; 
vencltartoX ,  vehcibar tni ,  Ve«*cl  xa«*:  Keai; 

F  r  *  )  v  e  1 ,  Siuevei,  Turrivel;  k  e  a  l  ; 

Ne^venclFarinx,  New  vencx  tar  tm  ,  NewvenclYa*:  Real; 
Ne^rrwdvei,  «ev»Si-je vel ,  wewi'ornVel:  Keai; 
i  in  e  1  ticriT nt ;  weal; 

f  rwoL'  ech  qst  ,  s  1  oeLecR  q  s  t ,  1'urnrecRqs  t ;  Keal; 

Lee  i  i cl e;  weal; 


P R 0 C t it OnK  CaictJewVenclFos  l 
v encl-ar tnx ,  vencitar tn x ,  Venciia*  :  Real; 

F  r  *  a  V  e 1 ,  o  i  u  e  v  e  1 ,  r  u  r  n  v  e  1 :  Keax; 

FrwaAcchqst,  b  1  de accR qs t ,  Turnt'CCKqst;  neai; 

var  Fe»'  vencxFarthX ,  Ne»>ieficltartni,  New  vencl  Ya^  :  Heal; 

v  a  *  Ne'wFrwdVei,  Newsiaevel,  n e *  1  u r r> v e  1 :  Real; 

var  i  in  einci  n-nt ;  nealj; 

C  ;J  fv  i?  I 

T  r  a  1 1  s 1 n  c  r  n n  t  =  0,4b; 

FotatJncrn.nts  0 . 0  J  b  b  2  5  ; 

V  A  H 

PrwoTime,  side  rime,  Turn lime:  Real; 

Averrwdvel,  AveSideVei,  MverurnVei;  Real; 
Aveiransvel,  AveVenclYaw:  Keal; 

TjrnlitqHau,  Turn!  no  arc;  Real ; 
'lransvelAnqwRjFartnCro;  Reni; 

1 ur n j nqCt r har th a ,  l urninyO-rLar tni :  Heal; 


FUNCTlCn  Hi n'LUSt Time  (. 
Initial  velocity. 
Acceleration , 


Distance:  heal) : 

Keel; 

VmP 

e'2frns4AC,  B2pls4AC:  Heal; 
L'  i  s  t  T  i  m  e :  Real; 


BlGIK 

Ir  Acceleration  =  o.u 

1  n  F  M 

Ir  initial  Velocity  =  0,0 

T  1  K  i» 

f  ini>J st l iii  e  :=  Pluslnf  lnitv 

t  tSt, 

a 1  n  i.'  i  s  1 1'  i  a  e  :=  a  c  s  i  Distance  /  lnitialvelocity) 

h,  u  s  c. 
buGlIv 

« ini.'ist'i  in.e  :=  Pi  us  Infinity; 

B^rrns4AC  :  =  Sqr  (lnitiaiwelocity)  +  2 •  0  *  Acceleration  *  Distance 
IK  c  2  i’.i  n  s  a  A  C  >  s  o ,  u 

T  H  L  ii 
bcG  1  iv 

Distfime  :=  -  initlalveiocity  +  bqRt  (P2tns4AO  /  Acceleration; 

If  (list Time  >=  0.0)  h.vd  t  List  fine  <  r ini is t lime  ) 
i  ri  F.  w 

Kinoistlm-essuistrime; 

Listiirre  :=  -  lnitialvelocity  -  oqnt  u?2irnS4MC)  /  Acceleration; 
IF  t  D  i  s  t  f  i  m  e  >  =  0,0)  «i«u  tDistline  <  h  inlist  lime) 

'i  ri  F.  I< 

ninlist'Iime  ;r  uistllwe 

; 

r*2iis4AC  :=  Sm  tlnitiaxvelocity)  -  2.0  *  Acceleration  *  Distance 

If  b2pls4AC  >=  0,0 
1  a  ii  .v 

B  i_  G  1  N 

tist’I  in  e  :  =  -  lnitialvelocity  ♦  £>qRt  tbycis4AC)  /  Acceleratiori; 

If  t D i s t T i rr e  >=  0,0)  Ai*u  CDistiine  <  .yinCistlime) 

T  of  i< 

M  moist  i  im e  :=  ui St Time; 

C.  i s t  J  i rr. e  :=  -  lnitial  velocity  -  SqRt  1.12P1S4AC)  /  Acceleration; 

IP  C  D  i  s  t  T 1  m  e  >  s  o .  0 )  a  r*  d  U<  i  s  t  l  i  it  e  <  k inlist  lime) 

I  HE  a 

nini’ist'Ume  :=  DistTime 


E  .*  [; 
E.’D 
Ei«D ; 


BEG  1  f'- 

Fr*aTirre  ;  =  Minbistl  lire  (FrwdVel,  Fr #aAccKqst f  Jransincrrrnt ) ; 
SloeTirre  :  =  MnDist  ri^e  (SideVel,  SideAccKqs t ,  Trans  Incrirnt) ; 
I u r n  1 1  rre  ;=  -inulst X e  (TurnVei,  I'urnAccKQSt,  Rocatlncrrnt); 

if  iFrwdlxu.e  <  =  SideTxme)  a.-w  iFrwdi ine  <  =  iurnlitfe; 

"i  nEr. 

1  lirelncrir.nt  :=  Fr*dTiire 

t  b  6  c. 

IF  (Sideline  <=  TurnTlme) 

Trip  . 

T  l  n  e  1  n  c  r  ir  n  t  j  =  i>  i  u  e  r  i  ir  e 

t  LS  r. 

1  i  rr  e  J  n  c  r  ff  n  t  :  =  i  u  r  n  i  i  ir.  e ; 

r»e*f  r  *ri  vei  :=  Fr*uvel  +  Fr*a*ccKqst  *  lifrelncrirnt ; 
kewsiae v el  :=  SineVei  +  SiaeMCcr\ast  *  Tin.elncrff nt ; 

Aie#iuinvel  :  =  TurnVel  +  TurnMccKqst  *  i imelncr*nt; 

Avetrwdvei  :=  (Fr*avel  ■*  Ne*r'r*ovei  J  /  2,  os 
Avesidevel  :=  (sidevel  ♦  He^ioevei)  /  2,0; 

AveJurnVel  :  =  (lurnvej.  +  Niewiurnvel)  /  2,0; 

.ve*  veuclYa*  :  =  vend  id*  +  Averurnvel  *  limeincriiint ; 

IF  i.  A  vet  rwd  *ei  =  o.oj  anB  c  Av/eSIoevei  =  u.oj 

ri  .it ' 

B  r,  G  x  f» 

Me* VenclParthA  ;=  veftd Ear tnX; 

.\e* v enclt-ar tnv  :  =  vendtaruiY 

tf.u 

t.  u.S  t, 

IF  A v e l u r n V e J  =  o , 0 

T.-it .. 

tsEGIN 

Ne*  v  end  Earth  x  :=  vend  Fart- da  ♦ 

AveFr*ovei  ♦  Cos  (Vencl¥a*i  -  AveSicievel  ♦  bin  (VehclYawj; 

e * v e n cl r. d r t h y  ;=  vencltartn\  + 

AveFr*a vei  *  bin  (VenclYa*;  +  Avebiaevei  *  cos  (VehclYa*) 

luo 
tbbw 
tic. Cl  M 

A  ve  iransv'el  :=  bq*t  (Sqr  (*veFr*aVei)  +  Sqr  C  AveSideVel ) )  S 

TurningKad  ;=  Ads  c  a ve'i rar.s V el  /  Avelurnvel); 

AvevenclYa*  :=  (VenclYaw  +  t«e»VendYa*’j  /  2,0; 

lransVelAnqwMTtartnc'rd  ;=  «ve»encna*  + 

A  r  ci  a  n  2  (AveFrwuVel,  AveSinevel); 


If  AveTurnVel  >  G.o 
TrJEw 
b  l  G  i  f; 


’I  urninqctrCartnx 
JarninoM-ad  *  Cos 
TurningctrCarth  i 
'iurninaKaj  *  Sin 


:=  vencjtarthX  t 

iTransveiAna  wK'lEar  thCrci  4  Pi  /  2.0); 
:=  vehcj-CartnY  + 

t,TransvelAngwKfc.arthCra  t  Pi  /  2.0) 


E:.P 
£  uS£ 

6  nG  1 1 


JurningetrFarthX 
Turning hao  *  Cos 
TurningctrEartn Y 
Turning Had  *  sin 


:=  vphcjLtarthx  + 
tTransVeiAng/<ki't.arthCra 
:  =  VencitartnY  ■» 
tTransveiAngwklEartnCrd 


Pi  /  2.0); 
Pi  /  2.u) 


P  ■  0 ; 


TurningArc  :  =  Aveirunsvei  ♦  1  in eincrrcnt  /  'lumingkad; 
Ir  AveTvirnVel  >  O.o 

I  :1 E,  ;'-i 
HnGlN 


ivewvehclFarthX  :=  TurningCtrFartnx  + 

lurningKaa  *  Cos  (TransVeiAngwKTEarthCro  -  Pi  /  2.0  + 

(wewVenclK.art.nY  ;=  TurnmuCtrt  ar  tnx  + 

)urninghaa  *  Sin  i.  Trans  v  elAnaAki  tar  tnCra  -  pi  /  2.0  + 


tuSc. 

bc-Gir 

new venclK artnX  ;=  TurnlnoCtrEartnx  + 

Jurnincjnaa  *  Cos  (.lransvpiAno*K'ltarthCrd  4  pi  /  2.0  - 
i'ew VenciLartnY  :=  TurninuCtrfiartny  + 

Turriingpao  *  Sin  tTransvelAnq*klc,artnCrc  ■*  Pi  /  2.0  - 

t  <r. 

C.ib 
t  :w  V  ; 


Kiii«cTiOf<  Ca ic VencHot ar  tnl  r ans «d t  r  i  x  ( 

Vend  t,  art  nX, 
vencl'-artnY, 
venclYawjweai; 

v  A  p  vehcl  ioCar  tnTransnatrix :  fransi^atrix) ; 

Hooiean; 

V  AK 

venclxldx,  venclYidx:  Integer; 

1  rr nP ts  int.artr.Cra :  akkay  1-2..2,  -i.,lj  Gf-  Ptirtar tnCrd 
r.i.crlrrriPts;  integer; 

su"tartnA,  SumEartni,  SuirtartnZ;  Heal; 

*eau£ar  thx ,  FeanFartni,  MeantartnZ;  keal; 
ouft^artuXtartuX, 


TurnlngArc) 
T  u  r  n  i  n  g  a  r  c ) 


lurningArc) 

TurningArc) 


d  o  n  k.  a  r  t  n  X  ca  r  t  n  V , 
d  jn  car  taXr.artnZ , 

Son-car  tnYcartnY, 
durcartnYcartnZ:  Real; 

Zxncercert#  FartnxCoef  t ,  EartnYCoef f ;  Real; 
x  /  cPt  .1  near  tnCrd ;  PtlnEar tnCra; 

Xvcr’t'iay:  Real; 


bEG  J> 

! 

Fu«  vehclileix  :=  -l  Ta  1  uu 
F./K  VebclxIax  :=  -2  lu  2  Du 
devil- 


Trr nPtsincart/iCrd  [veneixinx,  VehclYldxJ  IhartnxJ  ;  = 
Cos  denci*a*J  *  ** .  o  *  VehclXIdx  -  din  (VendYaw)  *  4 

I  r me usxncar taCro  Lvenclxlnx,  VenciYlaxJ  IF. a  r  t  n  Y  J  ;  = 
din  (  v  e  h  c  1 Y  a  * )  *  <* .  a  *  venclYldx  +  Cos  IVenclYa*)  *  4 

I I  ral-  tsincar  tnCra  Ivenclxiox,  VenciYlaxJ  ItartnZJ  :  = 

1 1  rnF  ts  ir.car tnCra  itfendXiax,  VenciYlaxJ  IFartrxJ , 

i  rraf'csmcartnCra  LVenclXIox,  VenciYlaxJ  ItartnrJ)? 

f  .1  ; 

W‘.,cr ir r nP ts  :  =  o; 

d n r,  ,.3rt n X  :=  u.o; 
dun  r art  a Y  :  =  u.u; 
d  o  n*  c  a  r  t  n  L  •  •  o  .  a ; 

F  u  k  vend  v  3  a  x  :  =  - 1  '(  0  1  0  u 
Fu«  vehclAlcx  ;=  -2  Tv  2  ju 

lr  (rirt-’tsInhartnCro  IVenciXJax,  venciYldxJ  ttartnZj  <> 
Tnh  \ 

Br-G  l  f 

i. ‘nxJrrnFLs  ;=  w»»rirrnPts  +  l; 
dan-cartnX  ;=  du.iifcartnx  + 

irrarcslncartnCrd  IVehclXldx,  VenciYlaxJ  LFartnxJ; 

'i u n  c a r t a Y  :  =  Su.rtartn  r  + 

7  rrnRtsint.artnCro  ivendXiux,  VenciYldxJ  lEartnjtJ; 
dun-cartaZ  :=  sa.i'Ear tr>Z  + 

T  r  i  m  ts  1  near  tnCra  [Vendxlnx,  VenciYldxJ  itartnZJ 

C  1*  l  / 


lr'  a.  ur  rr r ne  ts  >=  3 

Tat. 

HeG  I  l  • 


a  eariFartnz  :  =  SuntarthZ  /  wmtrTrrnHts; 

cuk  VehClYltix  :=  -1  TU  1  Du 
Fjh  venclAlax  :=  -2  lu  2  uu 
lr 

(.irrrPtslnFarthCro  IVehclXiox,  vendXIdxJ  ItarthZj  - 

T  iF.'j 
BeGU 


Vehcltartnx  -t- 
,0  *  vend Y lax; 
vencifcartnv  + 

,0  *  VendYIax; 

TrrnFtaa  c 


noDatReal 


MeancartnZ  )  >  2.0 


n  mfcr  Trrpf-’ts 


NdiDrTrrnPts 


-  l; 

Sunbartnx  :=  SumtarthX  - 

Irr nFtsiiu.artnCrd  IVehciAiax,  venclYIdx)  lEarthx); 
Sun"  tart  nvss  suits  Earth  Y- 

TrrnPtsintar tnCrd  IVehclAlox,  venciYldxJ  [Earth*)? 
Sun=.artnZ  :=  Sumhar tnz  - 

'J'r r nFcsmoartnCrd  IvenciAlox,  venclYIaxj  [Eartnz); 

TmifcsmtartnCro  iVenclAlox,  venciYlax)  [ Ear  tnz )  :  = 

E  <U 
E  v[  ; 

CalcvehclToEartnTransMatrix  :=  tNmDrlrrnPts  >=  J); 


1 e  talcvencilo Earth Trans  Matrix 
X  [  .« 

HtGI* 

veanharthx  :=  Sumbartnx  /  ivmfcrTrrnPts? 

►eantartfeY  :=  SuntartnY  /  .'•mtrTr rnpts ; 

veanEartnL  :=  SuncartnZ  /  "isbrTrrnPts ; 

Sa*c.artnAr artnX  :=  u,o; 
s  jit  car  thXtartnY  :  =  u .  u ; 

Surrcar tnXcar tnZ  :  =  u,u; 

SuncartnYtartnY  :  =  u ,  0 ; 

Sur bar tnY EartnZ  :=  0,0? 


:=  -i  TO  1  Du 

:  =  - 2  TO  2  o <j 


t-  jk’  vehcl  t  lax 
p  i  k  v  e  n  c  1  a  I  a  x 
It  irrnPtsInbartnCru 

’]  .  I K  f  I 

B  i-  G 1 1" 

SunEartr.xbarthX  := 
( irrnPtsInEartnGrd 
(irrnPtsInbartnCrd 
Sun  tar tnXbartni  :  = 
( irrnPtslnbarthCru 
(  t’r  r nPt  s I  nbar  t hCr a 
Sun  tart r,Xu.artn2  :  = 
(  irrnPtsJn Partner a 
(irrnPtslntartnCra 
SunbartnYearthY  := 
(irrnPtsIntarthCrd 
CirrnPts  in Earth Era 
SuntartnYtartnZ  ;  = 
(irrr.PtslnbartnCru 
(irmPtslnbarthCra 


Ivencixldx,  venciYldxJ 


sumtarthXKar tnx  + 
[VenciXldx,  VehclYldx) 
[VenciXldx,  VenciYlax] 
Suincar  thxEar  tru  + 
ivenciAldx,  venciYlax) 
[VenciXlax,  VenclYIdx) 
SuintarthXKartnZ  + 
LVenciXlux,  venciYlax) 
iVencxXlox,  VenciYidx) 
Sumtar cNr Ear tnY  + 
IVencrxidy ,  venclYIdx) 
[VenciAlax,  VenclYIdx) 
Suitvtar  thYEartnZ  + 
IvencrXlox,  venciYiax) 
iVencxxlax,  VenclYIdx) 


lEaitnZj  <> 


Ic.artnXJ  - 
ItartnX J  - 

l  E  a  r  t  n  X  j  - 
IbartnY)  - 

teartnXj  - 
itartnzi  - 

IbartnY)  - 
ttartnY)  - 

ItartnYj  - 
IbartnZj  - 


CalcVenc])oLartnlrans''atrix  [Sum earth XtartnX  <>  u.o) 


1 1  CeicVenclTo Earth  I tans Matrix 

TiiEw 
B  e  G 1 B 


CalcVencl 1 ofc.arthTrans Matrix  :=  U 


ivoGatKea  1 


N  o  U  a  t  K  e  a  1 


neaneartnX)  * 
*  e  a  n  c  a  r  t  r.  X ) ; 

meaneartnX)  * 
« eanbartnY); 

m  e  a  n  t  a  r  t  n  X ;  * 
fi;  e  a  n  e  a  r  t  n  Z ) ; 

m  e  a  n  E  a  r  t  h  Y  )  * 
MeaneartnY ) ? 

Meant art nY)  * 
w  e  a  n  t  a  r  t  n  Z ) 


Sun partnXLartnY  *  Sun EarthXEartnY  - 
SuiTt-artnXEartnX  *  Sun>c.ar  unYEartnY )  <>  o.O); 

IK  Calc venciTotar tnlransMatr i x 

TrtEiN 

0C.G1N 

tartdCoeff  :  =  ( 

SumEar tnXtar thV  *  Sunite»rtnxtar tnZ  - 
SuntartnXtartnX  *  Sun'Ear tnYLar tnZ j  /  [ 

SumEar thXEartn^  *  sun.EartnXt-artnlt  - 
SuncartnXcar tnX  *  Sun.KartnYtartnY  j ; 

EartruCoetf  :=  LSun.bar  unXcer  tnZ  -  EartnrCoetf  *  SumLar tnXEartnY )  / 
5umEar tnXtartnX; 

Zintercept  :=  Meantartnz  - 

LarthxCoetf  *  Meanbartnx  -  tartnYCoeft  *  keanr.artnY ; 

X vcPtinbar tnCrd  lEartnxj  :=  rteanEar thX  +  lu.o  *  Cos  (VenclYa*); 
x  vcPtlnEartnCra  Lc.art.niJ  :  =  weantarthi  +  iC.o  *  Sin  ( venclYay, ) ; 

X vcPt intartnCra  lEartnxj  :=  ^intercept  t 
fearthACoef f  ♦  x VcPt IntarthCrd  LhartnxJ  + 

LartnYCoeff  *  a vcPt inZartnCra  LtartnYj; 

XvcPtnag  :=  Sqkt  C 

Sqr  (xVcPtlnhartnCrd  LtartnXJ  -  weanEartfcX)  + 

Sqr  CxvcPtlnLartnCra  Lr.artnYJ  -  KeanEartnY)  + 

Sqr  lo.5  *  (X  vcPtlnEar  LttCrd  LEartnzJ  -  keantartnZ ) ) ) ; 

VehcllobartnlransMatrix,  kotat  [X]  LXJ  :  = 

CxVcPtlnEarthCrd  LEartnXJ  -  MeanE-arthX)  /  xvcptnag; 
VehclloEartn'iransMatrix.  kotat  [XJ  LYj  :  = 

i xvcPt. intartnCr-i  LLartnYj  -  weantar tm  )  /  xvcptnag; 
VehclTotarthTransMatr ix ,  kotat  [XJ  LZj  :=  0.5  * 

La  vcPtlnEartnCra  LEartnZJ  -  Mean  tart  hz  J  /  xvcPtMag? 

VenclloEar cn'l r ansrAatrix .  Kotat  [YJ  lxj  :=  -  Sin  (VenclYa*); 

Vend lotartnlrans Matrix,  xotat  LYJ  LYj  :=  Cos  (VenciYa*)? 

VenclTotartnTransriatrix .  kotat  LYj  LZj  :=  o.O; 

Venclloc-artnlransAatrix.  kotat  IZJ  i X u  :  = 

Vehcl'ioc.artnl  ranSMatrix,  kotat  [XJ  LYj  * 

vend  I  o  pa  rtn  Trans  Matrix,  kotat  [ij  [Zj  - 

venciropartn Trans Matrix,  kotat  [xj  IZj  * 

Vend iof.artnirans natnx,  kotat  LYj  LYj; 

V enclioEar tnT ransMatr ix  ,  kotat  LZj  LYJ  :  = 

vencliopartnTransMatrix,  kotat  lxj  LZJ  *  . 

vend  loc-artn  Trans  Matrix,  kotat  [YJ  [Xj  - 

vend  iOu.artnTransMatrix ,  kotat  [xj  lxj  ♦ 

Venclxot-artniranskatrix,  kotat  [Yj  iZj; 

V end i or.ar t nlr anSMatr i x ,  kotat  LZJ  LZj  :  = 

Vend Totar tnT rans Matrix .  kotat  lxj  LXj  * 

Vendiohar tnTransMatr ix ,  kotat  [YJ  L Y j  - 

vend loEartn Trans Matrix,  kotat  [aj  [YJ  * 

V  ehcl  j  ot.ar  tnl  r  ansna  tr  1  x ,  kotat  [Yj  t  X  j  ; 

Vend  joEartn  Trans  Matrix,  Irans  LXJ  :=  VehdtartnX; 

V  end  lobar  tnT  r  ans  Matr  i  * ,  Trans  LYJ  ;s  VendEartnY; 

Vend  I opart m ransMatr ix,  Irans  [ZJ  :=  VenclAitituae  +  Z intercept  + 
E a r t n x C o e f f  *  venclbarLnx  +  EartnYCoeff  ♦  VehclEartnY 


'  t£  ix  0 

•  t  .  I' 

E  :v  0 
t  » u ; 


b LG  IN 

CalcPosije-jQtE  rdu*EromiransMatrix  i 

venclt’osfcequf f  ran:*MfcarthCrd,  vend Tot.artrriransNiatrix)  ? 

vendi-artnX  t  =  v endi-os ueqO t E  r up x RTOar  t hCr d .  lrans  Lx]; 

vendr.ar  tnY  :=  * enciEosueyOtE  rop* KTtar  ihCra.  lrans  U'J; 

Vend/a*  :  =  V'endPosoeqUf  E  ra.n*KT.e,ar tnCra.  hotat  iYa*j? 

Er*dvel  :  =  v'etidLinvellnvendCra  IVenciXJ; 

Sioevel  :=  vendLinveilnvendCrc  IV enc  1 i  j  ; 
lurnvel  :=  v'enclAnqVeilnVerciCro  ivendZj; 

v  end  .idyl  r  a  j  •  waxbdysttldx  :=  u; 

v/encl.'dyi  ra  j  ,  Vehddaybtts  t  venddrty l ra  j .  MaxtdySttidx) ,  ripe  :  = 
vencliidyi  raj .  Vendooybtts  iVenclocyira  j  *  MaxtayStt lax) . 
vercl  iCLdrtnirans.iatrix  :=  VenciXot  ar  tniransMotrix; 

CdicVenclbayirajjs’irue? 

i> 

IE  r  •»  o  v  e  i  <>  n.i'j  UR  ISidevei  <>  t.i)  uk  ilurnvel  <>  0.0)  OR 
l r r *o mCck y s t  <>  0,0)  ur  (oideAccKqst  <>  o,o)  CR  CiurnAccHqst  <> 

1  rt  t 


v end rtdyl r a j ,  MaxbaySttidx  :  =  vendbdyTra j .  Max<aayStt  ldx  +  l 
Ca  1  er  e*  vendPos  ( 

vendc.art.vX,  vendtartr.Y,  »ehclYa*, 

E  x  '* a v e  1 ,  didev'ei,  lurnvel, 

Fr*ciAccKqst,  biaeAccHqst,  lUrnACCKqst, 

ive*  veMclEiar tnx ,  he*  vencltarthY ,  ive*vehdYa*  , 

<v  e  *  r  r  x  a  v  e  l ,  n  e  w  s  i  o  e  v  e  l ,  u  e  *  'l  u  r  n  v  e  1 , 
i  m  elncrn-nt ) ; 

venclodyiraj,  VehcldoyStts  ivenclbayira).  tfaxedySttldxJ,  T i n= 
Venclddy'j  ra  j  ,  VehcluoyStts  t  Vehclrsaylra)  .  EaXocybttldx  -  1). 
linelncmrit; 

CulcVenclbdyTraj  :=  Cdlcvencllot.aruiiransi'atrix  C 
Ng* v enclt  ar tnx ,  ive*  vendtarthY ,  Ne#venclia*  ( 
vencl^dylraj,  VenclndyStts  iVehclbayiraj.  R axed yStt lax) , 
Vend iotartn Trans  matrix); 

e  r  *  j  vel:=\ewFr*'dVel; 

Sioevfcl  :=  .»e*SioeVei; 
luraVel  :=  ne*Tarnvel; 

Ver>clE.artnX  ;=  ,\ie*vende.arinx; 
verdr.artnY  :=  t\e* Venclfc-ar tny  ; 


Currntl'in-e ; 


I) .  0 ) 


i  i  iti  e  t 


Venclla*  ;=  vend  i  a* 

U  -ilit.,  ir.oi  Caic'venclbdy  I  ra j  j  uh  ( ( venciwayTrej . 

vehclodySLts  l  venclody i  ra  j  .  "axadysttlcix]  .  linie  -  CurrntTine)  > 

Gold Alar ton  txecintrvl ) ; 

If  Calc v enciHdylra j 

’i  H  K  • 

if  ( F  r  »  a  v  e  1  <>  o.U)  UK  ( s  i  d  e » e  i  <>  0,0)  OR  (lurnvei  <>  0 ,  u ) 

I  fit  S' 

Bc,G  i  M 

Ip 

(ads  (FrvujVel)  >=  Aus  (Sidevel))  am> 

CtAus  (Fr*dvei)  /  vend  Maxi  r  ansAcc)  >  = 

(Aps  (Turnvel)  /  venclMaxKotatAccv) 

I .  i  £ .  • 

H  i_  G  I N 

f  r*oDecK'ist  :=  -  venciMaxiransAcc  *  Fr*avel  /  Aps  (fr*dvel); 

SiaeOecKqst  :=  -  vencli’iaxlransAcc  *  sioevei  /  Acs  (Frwavei); 

T a r n o e c k q s t  :=  -  vencltfaxKotdtAcc  * 

(lurnvej  /  VenclMaxKOtatMcc)  /  (Aus  (Fr'Aavei)  /  VehclMax'fi  ansAcc) 

K  .U 

Ir 

((A os  (Sioevei)  /  v  e  n  cl  *  a  x  1 1  a  n  s  A  c  c )  >  = 

(«ts  (TurnVel)  /  VenclFaxkot.atAcc)) 

1  'iCn 
brJGl  n 

FrwdDecKqst  !=  -  venclKaxi ransAcc  *  F r w o V e 1  /  Ads  ( S  i  q  e  v  e  i ) ; 

SideuecKqst  :  =  -  vend  ^  ax  1  ransAcc  *  sioeVel  /  Acs  (Sidevei); 

lurnDecKqst  :  =  -  vencl*axKotatACc  + 

Ciurnvel  /  venciMaxkotatMCC)  /  (Ads  (Sioevei)  /  veficlMaxl ransAcc) 

t  ..n 

tbSL 
t G I  f 

Fr  *•  jpecKqst  :=  -  venclNax  Ir  ansAcc  * 

(Fr*dvel  /  VenclMax  I  ransMcc)  /  (Ads  (lurnvei)  /  venclMaxRotatAcc) 
SideDecnqst  ;  =  -  vend  Max  I ransAcc  * 

(Sioevei  /  Venci i* ax r ransAcc)  /  (Acs  (lurnvei)  /  VehclMaxKotatAcc) 
1  urnuec.kqst  :=  -  Vendpaxkouat Acc  *  Turnvel  /  Acs  (lurnvei) 

L'<V; 

J  p  p'  r  *  d  V  e  1  <>  O.o 

Decline  ;=  Aps  (frvvaVel  /  FrvvdDecKqst) 

PU  S  L 

1p  Slaevel  <>  0.0 
Tnfx 

Decii.i.e  :=  ads  (SldeVel  /  hiaet'ecRqst) 
t  l  5  D 

Decline  :=  ads  (lurnvei  /  iur nneckqst )  ; 


Kc,f  cA  1 


VehclddyTra  j  .  BaxriaySttlax  :=  venclbdyTr a j .  RdXbdySttldx  +  1 ; 
CalcNewvenclPosC 

VencltertnX,  Vencltar tni .  Vend Yaw, 
t r * d V e 1 ,  sioevel,  Turnvei, 

R rwaUecRast ,  SideuecRqst *  1 urnDecKqst , 

New venciEarthX,  New venclfcar thY  ,  NewVenclYaw, 
n  e  w  f  r  *  a  y  e  l ,  a  e  w  5  i  a  e  v  e  l ,  w  e  *  X  u  r  n  V  e  i , 

1  irrelncrmnt)  ? 

VenciBdyTr a j .  V ehclBoySt us  tVenciBdyira j  .  KaxadyStt lax]  .  rime  :  = 
VehcloayT r ej  .  VenclBoyStes  iVenclbayTra j  .  f'axodystt  idx  -  l).  Time  + 
Tinelncrmnt; 

CdicVehclbdyiraj  :=  CaldehcriobarthTransRatrix  ( 

New  venciLar ihx ,  New  vehcltar  thy  ,  New venciY a* , 
venclodyTra j . 

VehclDd^Stts  IVenclbayTra  j  ,  R'axbdyottldx  j  .  VeriCiTofc  artLTrans’-atr  ix) 

f  r  *  'i  v  e  l  :  =  n  e  ■*  l  r  *  d  v  e  1 ; 

Side V el  :=  NCwbioeVel; 
lurnvel  :=  NewJurnVei; 

vehclt-artrix  :=  NewVenclLartnx; 
v e r. c  1 1 a r t n Y  :=  ,\e*vencltartny; 
vend  Yaw  :=  i<e*  YencxYaw 

UiiiL  iwOl  CalcvenciBdyTra  i  ]  OR  ( i Venclbdyl raj  • 

VehclbdyStts  i Venclbdyl raj .  ^axBdySttldx ] .  Time  -  CurrntTime)  > 
duinAlqrthinKxecIntrvi  +  uec’iirrej] 


&•.(> 
t'.b 
L  .  n  ; 


r'oNC  ilG-  CaicvenciLegs'lrajs  C 
v  e  n  c  1  no  >  '1  r  a  j  :  v  e  n  c  1  od  y  1  r  a  j  T  y  pe ; 
vencli.e  jsotts  i  venciLegssttslype; 
vencloe  vscrrnas:  Vehcluey sCmnos l’yue; 
v '« h  Venclpegs  irals :  v enclbegsl  r a  i slype ) : 
t-  o  o  i  e  a  n  ; 

v  \  r. 


t  utorevenclneqsstts:  VeiiclbeusSttslype; 
Leolrajlax:  ARRAY  [Legs]  tit  jnteoer; 
LeQsOatofLinitsldxs:  a  k  k  a  Y  [pegs]  uK  Integer; 
F  a  t  u  r  e  T  i  n>  e :  Real; 

RotentialvericiLegsStts:  vehciLeyssttsType; 

Leg sC an neLl t ted ;  booiean; 
ciiriuegGjtOfLifiitsidx:.  integer; 

Leg  tinLegOu to f Limits  :  Legs; 
ve n c 1 u eul raj :  VencltegTra] i y  pe ; 

Leg;  Legs; 

YenclCralox;  veitCICrd? 
t ar tncroiax ;  tartnCra; 
buyottiax:  integer; 


Function  vencilsStaole  C 


Vend  J'o^artnTra  ns  Matrix:  Transnatrix; 
vencl^egsotts;  venclLegsSttsiype): 
boolean; 

CuNo'l 

0  instability  v-argln  =  0,5; 


V  A  K 

vehclCGIr.LartnCra:  F t inter tnCrd; 

F tl oCGCstnc,  Ft l oCGAny le :  neal; 
t-  t  J  oCGStaoMronArc:  Keel; 

F 1 1  oCGAno  lePls  ,  F t 1  oCGAnctle^ns :  Real; 

F tToCGAny i ePl s Ts t ,  Ft ToCGMngie^ns'i  st ;  Boolear; 
I stol neSlope :  Real; 

Leg,  utnerLey:  Legs; 


BtGi'v 

'1  r  ns  f.  rn  Pt  lotar  tnCroF  r  venclCru  (. 

VenclCGlntarthCrd,  vencllobarth Trans Matrix ,  VebclCGin VehclCrci ) ; 

Ve.n  ells  Stable  ;=  True; 

F u R  Lea  :=  p  t  l<  t  Tu  k  r  « t  Du 
it  venc  UsStaole 

InK  f 

Ir  venciLeasStts  ILegJ,  Sptot-t  =  Support 

Inr 

bwG  if; 


FtioCoLstnc  :=  SqKt  ( 

J  IT  ( 

vend CGlnt art hCro  [fcarthxj  - 

vencluegsstts  lueyj.  Pos lnt-ar tncro  Ltartnx]]  + 
s .{ r  ( 

venclCG intartnCrd  itartnyj  - 

v end uegsst ts  [Legj.  Posint arthCr J  [EarthiJ))? 


if  FtloCGustnc  >=  ninotaoi-iityFargin 

1  r,  h  .M 

B  i  G 1 P 

PtloCGAnaie  :=  A  r  c  T  a  n  2  C 
VehclCGintartnCrd  ICartnAj  - 
venclLegsstts  ILejI.  PosmtartnCrd  CLarthXJ, 
VenclCGintartnCra  (tartnij  - 

venduegsstts  (Legj,  PosmEar tnCrd  IfcartnrJ); 


P  1 1  oC gs l  a u w r g n A r c 

P  t  ’i  o  C  G  A  r>  Q 1  e  P 1  s  ;  = 
IF  r  1 1  0 C G A n g  1  e F 1  s 
T  .i  fc.  ,i 

F ui oCGAnoiePls  := 
FtToCGAnglePlslst 


:=  Arc^in  (Minstanili ty^argin  /  FtToCGustnc) ; 

FtioCGHngle  *  F tloCGStaoKrqn Arc; 

>=  J.O  *  Pi  /  2.0 

F t 1 oCownglePis  -  2,0  ♦  Pi; 

:=  False; 


If'  Cos  CFtfoCGAnglePlsj  <>  0,0 


1  1  F.  n 
bEGl  N 


IstLineSlope  :=  Sin  ( F  l’1  oCG Any leP is )  /  Cos  (rtioCGAnglePls) 

IF  Ft loCGAny lePls  <  Pi  /  2.u 

THEN 

BtC-lb 

fur  utheroey  :  =  FtLt  ro  PrKt  do 
IF  OtherLeg  <>  Leg 
I  HEP 

Ir  vencibegsstts  lOtnerLeg].  Sptstt  =  Support 
'ir if;,, 

F t ToCGAnglePls l st  :  =  r tioCGAnyiePls ist  OP 
( v enclLeosStt s  lUtnerLeyJ.  Poslntar thCrd  LtartnY]  >= 
Vencltegsstts  iLeg]  .  Posl'ntartnCro  LEarthYJ  + 

Tstbinesiope  * 

( venclbegsStts  [otheruey]  .  Poslntar trcrg  IFartruj  - 
VehclLeysstts  tuegj.  Poslntar tnCro  itartnXJjj 


t  L) 
EuSE 
BEG  IE 


FuK  OtherLeg  :=  FtLt  10  Rrmt  uu 
.  IF  OtherLeg  <>  Ley 
1  rl  F.  i. 

IF  v enclLegsStts  tutneroegj.  sptstt  =  Support 
Tatw 

F tl oCGAny leP lsi st  :  =  F tl oCGAnulePls i st  OR 
( venciLegsSt  is  lutneri.eg]  ,  Poslntar thCrd  UartnY]  <  = 
VehclLeysstts  luegi.  Poslntar tnCro  ifcartnYl  + 

1st uineSlope  * 

( venclLegsStts  IGtneiLegj.  Poslntar  thCrd  IfcarthX]  - 
VehclLeysstts  ILeyJ.  Pos Jntar tnCro  UartnxJj) 

Fi.D 
t-.o 
t  u  S  r. 
dEGit 

Ir  Sin  i F tloCG AngleP 1 s >  >  O.u 
In  ti. 

B  tG  1 1" 

fop  OtherLeg  :=  FtLt  10  RrRt  DO 
IF'  Gtneroeg  <>  Leg 

T nr 

IF  v enciLegsStts  [Utner Leg] .  Sptstt  =  Support 

1.1  h; 

F  L 1 o C G A n 3 1 e F 1 S T s t  :  =  F t 1 oCG Ang leP 1 s i s t  0R 

(venclLegsStts  [CtnerLeyl.  PoslntartrCrd  UarthXJ  <  =  O.o) 


t  /  P 
ELSt 
B  t  G I  e 


FOP  OtherLeg  :=  FtLt  10  Rrnt  DC 

IF  Ot ner Leg  <>  Leg 

THE., 


IK  venclLegsStts  LUtneiLegJ.  sptstt  =  Support 
TriLu 

Y  tToCGAnglet'ls  1  st  :=  Ft  loCGAnglePls )  st  ch 
l  venciLegs&tt  s  [UtherLeyJ  ,  Poslntar  thCrd  [harthXJ  >=  o.o) 


tuD 

f.  u  o  ; 

f  tToCGAfiQie^ns  :=  ftIoCG«nale  +  F tioCGStacMrQnArc; 
IF  F  1 1 0 C G A n y 1 e K n S  <  -  Pi  /  2.0 
T  ri  F  P 

FtloCGAnqleP.ns  :  =  FtloCGengleKns  +  2,0  *  Pi; 
f tToCGAngletfnslst  :  =  False; 

IF  Cos  (F tioCGAngieMns )  <>  u.O 


7  ri  t  •< 

B  t,  G 1 N 


TstLineGlope  ;=  sin  (FtioCGAnylefons)  /  Cos  t,FtioCGAngie»ins) 
IF  Ft loCGAnyler.ns  <  Pi  /  2.0 

1  d  F  ,v 

Bt-Gil- 


Fup  utnerLeg  ;=  FtLt  XL  Pr«t  DU 

IF  utnerLeg  <>  Leg 

Int;, 

IF  venclLegsStts  LOtnei Leg J  ,  SptStt  =  Support 

1  r  1 1*.  ,4 

F tToCGAngie.-'nsTst  ;=  Ft'ioCGAngieMnsJst  up 
( V enci Leoss  1 1  s  tctheibegj.  Posintar thCr d  IKarthi)  < 
VehclbegSot ts  ibegj.  pos l ntar tnCr a  itarihiJ  + 

J stLineslope  * 

(VenclLegsStts  (LtnerLegj.  PoslntarthCra  ItarthA]  - 
venclLegsStts  ILegj,  Posi ntartncra  LEarttxJj) 


fc  » L 
F  uS  f. 
HlG  ii‘ 


for  Other Leg  :=  FtLt  )C  Rrht  DC 
It  utnerLeg  <>  ueg 

1  ,i  r*.  ; 

if  venclLegsStts  lutnerLegj.  sptstt  =  Support 

j  h  t  ,V 

FtToCGAngle'-ms'lst  :=  FtXoCGAngleMns  i st  oR 
(VenclLegsStts  LGtherLegJ .  Pos I  near t rcr d  tEarthYJ  >  = 
v  ehc  l  Leg  sst  t  s  Ibegj.  Pos l ntar thCr a  lEartM]  + 
TstuineSlope  ♦ 

(VenclLegsStts  lOtnerLeyJ  .  Poslntar t.ncrd  ItarthA]  - 
vehcluegsSt ts  LLegj  .  Pos 1 ntar tnCru  IfcarthXJj) 


Ef-L 
F  .,D 
tbSf. 
BKG1N 


IF  Sin  (FtToCGAn gleans;  >  C.O 
T  ri  F  N 
Be,  GIL 


FOR  OtnerLeg  ; =  ttLt  TO  RrRt  DU 
It  OtnerLeg  <>  Leg 

T  H  L  f 

Ir  venclLeasStts  [OtnerLeyJ  •  SptStt  =  Support 
TriEl. 

FtloCGAngle insist  :  =  FtToCGAnolewns 1st  uR 
l venciLeosStt s  [OtherLey]  .  RosinEarthcrd  ILartnx]  >=  o,n) 


h  ii  0 
e  lSE 
ec.ul  N 

FOR  Other Ley  J=  FtLt  TO  RrKt  DL 
Ir  (JtherLeg  <>  Leg 

I  H  FI  N 

It  venciLegsStts  lutnertiegj  .  sptLtt  =  Support 
T  n  t- .« 

Ft'J  oCvjAngle.'insTst  :  =  r  t  Toll Anglehns I st  UR 
(veficlLeqsStts>  ICtnerLeyJ,  Rosin Eartrcro  IrartnA]  <=  o,u) 


ti-.U 

E  -\i  l'  ; 

v'er.cilsbtaDle  :  =  FtroCGAnyiePlsist  mnu  r t  loCGAng i eonsist 

t  ,D 

E  ,'i  L 

6.  0 ; 


Fui'CT  1C .*  Legf  oothol-JF  oond  C 
Currntrtaysttlgx :  Integer; 
Vendddylraj:  Vend boy ira J1 ype ? 

Leg:  ueys; 

uegPosinEartncrd:  PtlnEartnCrd; 

vaR  vehclLeglra  j  :  VenclLeyl  ra j  i'ype ) : 

rooifdn; 

C  U  i'i  b  i 


k  dx  v  endX  Y  IcJx  =  4 ; 

MrwericlXidx  =  -  4; 
t. ax  vend X idx  =  l; 
MnvenciYldx  -  -  2; 
xax  vendY  ivix  =  1; 


V  Ak 

CurrentTiine:  Real; 

N.f.PlLxtFhdlnVehclCro,  PttlwxtFhdlnvenciCro:  EtlnVendCra; 
CurrentFhoInfcar thCra ,  PttlwxtF  rdnt,ar thCrc :  FtlnEarthCra; 
LegKtnFartnxust,  LeyRtnEartnYOst;  Real; 

LeqntnRarthXYLst:  Real; 

LeqKtnKar thX Ylntrvl ,  LegRtntar  t.  nx  Y I  nt  rvls  :  integer; 
LegktnEartnxYOstlnc:  Real; 

LegRtnEortnxi>stInc ,  LegRtnc-artnYLstinc;  Rear; 

LttElev,  Plctiev:  Real; 

EarthxPos,  EartnYPos,  Eartnzpos:  Real; 

TstcorrencFncUntarthCrd,  TstPtti»vxtFndlnEarthCrd:  PtlnKar t  ncro; 
vehdXYIox,  VehclAlax,  Vencniox;  integer; 


FUNCTION  Future.bdySttldx  ( 
vencloayTraj:  VeOclBuyTrajiyue; 
Futurelime:  Heal): 

Integer ; 

B  t  G  i  h 

FutureBdysttlax  :=  u; 


Aiilbfc 

(FuturendyStt Idx  <  vencldavira;) ,  MaxBnyittldX  )  AnU 

CrutureTime  >  VehclHayiraj .  VenciBoyStts  iFuturetayuttldxj .  line)  Du 
F utar ebdyst tlax  :=  F utur eBoystt lax  +  l 


tsCGiN 


Current  lime  :s  venclbaylrd).  vencibdystts  [currntcaySttldxJ.  line; 
CAGi.  L-ey  OF 


F  t  L  t ,  F  t  H  t  J 

NnniNxtFnalnVehclCrd  IVenciA) 
C  x  l  t ,  C  r  H  t : 

N.nniNxtFholnvehclcrd  CVenciX) 
k  r  L  t ,  R  r  R  t : 

tv  .t-  n  i  h  x  t  F  n  a  I  n  V  e  n  c  1 C  r  a  [  V  e  n  c  i  X ) 


c  .  r*  • 

C,  4«  L'  f 

CAul  i e  .3  OF 

Ftlt,  CrLt. ,  HrLt: 

It  neg  =  C  r  u  t 

T  ,i  F'\| 

n n  n i tv x t F  halnvehclCi  d  Lvehci  Y ) 
h  L  S  r, 

ix  .n  r.  i f  x  1 1  n  a  I  n  v  e  n  c  1 C  r  J  [  v  e  n  c  1 Y  J 

F  L  k  L  ,  C  r  H  t ,  Hint: 
it-  Ley  s  C r n t 
lHr« 

:•>  1. n i r<' x t F h o T r  1 V e n c  1 C r a  [Vencn  J 
t  lS  t 

N  .1  n  1  \  x  t  F  n  a  1  n  V  e  h  c  1 C  r  d  tVencii) 


venclctr icvenciF tLegs  +  2,u; 

VenclctriovehciCrLeas  +  2,0; 

venclCtriovencinrOeas  +  2.0 


venclCiriovehciLiLegs  +  1.2S 
vencictriovenciLtLeys  +  u,t>2b; 

venciCtrioVencincLegs  -  1 . 2  S 
vend  c  trio  vencmt  Legs  -  u.o2S 


F  i  Li ; 

NnnlNxtrhdl nVehclCra  LVehclZj  :=  O.n; 

Lear  not  rioldF  ouna  :=  False; 

Fuk  VenclXYIdx  :  =  0  TO  Maxvenclx* lax  DO 

F'uk  VenclYlax  :=  -  VenclXYIdx  TO  weDClXYlax  CO 

it  (.Ads  (VenciYiax)  =  VehciXiiax)  a.^u 


UenclYidx  >=  MinVencliiax)  m.nd  (Veticliiax  <=  Max vehclYIax ) 

1  n£  j 

FjP  VehclXlax  :  =  -  VenclXYlax  TO  vencixYldx  D(j 
It  (.Aos  (y/enciXidx)  =  Yehclxiicix)  And 

ItfenclXIdx  >=  Min venclAidx j  hnd  tvenciXlax  <  =  Max venciX idx ) 

1  lit!' 

IF  vui  LegF  ootholoF  ound 

1  Ht'v 
BhG  JL  f- 

p c t i*1  x t Fhol n VehciCr a  [VencxxJ  *  = 
ivuiniiMXtFnoInVenclLra  ivencxxj  +  vencixidx  *  1.0; 

Ip  dcui  (Ord  Cleg)) 

T  iltn 

PLtlf<xtFhdlnVehclCrd  iVehcxYJ  :  = 

wf-nlF xtFnalnVehclCrd  tVencxY]  -  venciYlax  *  l.c 

LliSc- 

PttlNxtFnalnVehclCrd  tvehcxiJ  :  = 

K’.iniNxtFnalnVenclCra  [VencxY]  +  venclYldx  *  1.0; 

P  c  t  i  i\  x  t  F  r.  a  I  r  V  e  n  c  1 C  r  q  CVencxZ]  :  = 

N  hi  n  j.  ij  x  t  F  n  o  I  n  V  e  n  c  1 C  r  -j  i  Yencxz  j ; 

imsfrrrPtiotartnCraFrVencixJrQ  (PttluxtFhdinfcartnCrd, 

VenclFdy'J  ra  j  .  VenciddyStts  IF  uturetsaydtt  lax  C 

V  encl  bdyTra  j  ,  Cur  rentlinie  +  Legntniinjj, 

Vencllotartnlrans matrix,  PLtlxxtFnuinVehclCraj; 

Ptt ANxtFnalnFarthCru  iLartnZJ  :  =  irroFtnd  C 

Ptt A\xtrha] nfcartnCra  Itartnxj ,  PttlNxtFndintartnCr d  tfcarthYJ); 

If  Pt tlNxtFnaintartnCrd  ifcartnZJ  <>  NoDatPeal 

1  ri  ti  ■< 

Ir  Ftlnbin. its  (Lej,  PttiuxiFhainhartnCrd, 

VenclaoyTra j .  VehcindyStts  [F uturebaySttlcx  l 

V  encl  nayl  raj  ,  Cur  rent  rime  ♦  Leoktn  l  irr )  j  , 

Vehcl  lotartnTrans^atrix) 

(  t  p  n 

Cur rentF holnKar tncrd  Lcaxtnx]  := 

^ehclze  isi>tts  iLegJ.  PosxnfcarthCru  ttartnxJ; 

Cur rent f no  I ntar tnCrd  [Karthi]  :  = 
v encluegsbt ts  luegJ,  PosmLartncra  [fcartniJ; 
CurrentFhalntartnCrd  lc.aitr.Zj  :  = 

VfchclLeysStts  tLegl.  PosxnfcarthCru  ifcartnZJ; 

LegKtnLarthxOst  := 

PttlHxtF  n  din  Farther  -J  IfcaxthXJ  -  CurrentFnoInfcarthCra  ifcartnxj; 
LegKtnE.artnYDst  :  = 

PttiNxtFhal nfcartnCra  ItaitnY J  -  Currentfhdlnfcartncrd  itartnlJ; 
Lea Ktn tart hxYDst  ;= 

SgKt  tSqr  tuegkintartnxust)  ♦  £>-.jr  CbeyPtntartnYnsL)  J ; 
lr  L.egPtnt.ar tnx  i ust  <>  u.o 

I  n  t.  - 
BCG  IM 

l egktnFarthXY lntrvis  i=  o ; 


REPEAL 


LegktnEartnxY Intrvls  :=  Legntntar thx Y intrvls  +  1? 

LegktnEartnxY ustlnc  :=  Leghlncarth* H-st  /  LeaktnEarthXYintrvis 

U i-i TIL.  LegktnLartnXYDstinc  <=  0.5; 

LeqKtntr  arthXDstlnc  :  = 

LegktnEartnxYUstlnc  *  uegktntar tnxust  /  Lean tnEar tnXYus t ; 
LegktntartnYLstinc  :  = 

LegktnEartnxYustinc  *  uegktntartnYDst  /  LegRtnfcartnxYDst; 

Lttclev  :=  CurrentFhdlnc-artnCrd  ItartnZJ, 

Flctlev  :=  Pttlnxtf nolncar thCru  (EartnZj; 

FUR  LegktnEarthXY Intrvi  :=  1  Tu  (LegRtntarthX YI ntrvls  -  1)  uo 
8EG1M 

EartnxPos  :  =  turr ente  ndlntar tnCr a  Itartnxj  + 

LegntnEar thx t lntrvl  ♦  LegRtntar tnxDst me; 

EartnYPos  ;=  Currentr naintar tnCro  LtartnYj  + 

Legkt nt'arthXYlntrvl  *  LeoKtntar thYLstinc? 

LartnzPos  :=  Irrntiev  ttartnxpos,  tartnYPos); 

IE  ilartnZPos  <>  NoUatKeal 
T  rl  E . 

If'  iFarthZPos  >  Lfttiev)  A k U  (EarthZPos  >  PicElev) 

Tut  h 
BEGIN 

Lfttlev  :=  tarthZPos; 

Plctlev  :=  tartnZkos 

b  u  0 

ELSE 

IF 

barthZPos  >  (Ltttlev  +  (Plctlev  -  Lfttlev)  * 
LegktnEarthxYlPtrvi  /  LeaKtntartnXYintrvis ) 

1  rt  L ,  ;n 

IF  tarthZPos  >  Ltttlev 

TntiY 

Ltttlev  :  =  Plctlev  +  (.LartnZPos  -  Flctlev)  * 

LeaktnEar tnxY lntrvls  / 

( begR  tntartnX  Y  intrvls  -  LegktnEartnxY  lntrvl  j 
El.Sl 

Plctlev  :=  Lfttlev  +  (LartnZPos  -  Lfttlev;  * 

Legnt nEarthXY Intrvls  /  LegKtntartnx  rintrvi 


to  d  ; 


IstCurrentFndlnEartnCra  :=  CurrentFhd I ntartnCrd; 
TstCurrentFndlntarthCra  ItartnZJ  :=  Lfttlev  +  0.5; 

TstPttluxtFncJlntartnCrcj  ;=  PttinxtFhointar tr.Crn; 
TstPttl.^xtFndinEartnCra  LtartnZJ  ;=  Pictlev  +  u,5; 

L.egF ootholdFoand  ;  = 

FtlnLimits  (Leg,  XstCui rent! hointar thCrd  , 
VehclBdy'lraj.  venclBdyotts  IrutureBdyottlux  ( 


VehclBayTra j  ,  Currentlime  +  LegLitTirr)]  . 
VehclTobartnTransMatr ix )  and 
F  tin Limits  (Leg,  TstPttlNxtFndintarthCro  , 

VenclbdyTra j ,  venclboybtts  [F uturebayottlax  ( 
VehclBdyTra  j  ,  Cuirentfime  +  LegRtnl’lrr  -  LegPlcTini )  ]  , 
venclxot-arthTransMatrixj; 

IF  LeyFootholdKouna 

7  ii  t ,  N 

wlTn  venciLeaTraj  DU 
BEGIN 

Lftiime  :=  Current!  i'»e; 

Lttrigt  :=  fstCurrentrhdinEartnCrd  1 1  a  r  t  h  Z  J  - 
Curr entFtidlntar tnCrd  LEartnzj  ; 

Picii.ne  :=  Current!  ime  +  LegKtnliip  -  t,egF  IcTim? 
Irnsfrmptl’o  venclCrdKrEarthCrd  (PicPcsinvenclCrd, 
Vend saylra j .  venclbdyStts  (FutureDuySt t ldx  ( 
VenclbdyTra j  ,  Currentlime  +  LegRtnTin  •  LeaPlcTirr)  J  , 
vehclioEartnTransMatrix,  pttlr«x IF ndinEartnCrd  j ; 
Cttiime  :=  currentlime  +  LegRtnTin. ? 

CttMgtMin  :=  rstPttlwxtFnuintar tfiCru  UarthZJ  - 
PttlNxtFhdlnEartnc'rd  ItartnZJ  ; 

CttHgtddX  :=  i  stPttl^xtF  ndintartriCro  [tartnz]  - 
Ptt lNxtFhdlnt ar tnCra  [EartnZJ ; 

NxtF nulntar tnCrd  :=  PttlNxtFnulntartncrd; 


E  n  D 
KmO 
EnC 
e  .'4  n 
EnD; 


FDNCliOw  LegOutuf Limi ts idx  ( 
CurmtBdybttlax:  integer; 
verclbdylraj:  VehclBdyirajlype; 
Leg:  legs; 

LewPoslntarthCro:  PtlnhartnCrdj; 
integer ; 

V  AK 

F tUututLini ts :  boolean; 


begin 

LeguutOtLlrrltsIdx  :  =  CurrntuoySttlax  -  1; 


K  t  F  Z  A  l 

LegDuLOf  Limits  Idx  :=  uegUuuutLiir.itsIdx  +  l? 

f  tOututLJ  rrits  :=  njT  Ftlnbinilts  ( 

Leo,  LegFosinhartnCrd, 

Venclbdylraj.  VehcinayStts  ILegOutQfLinitsJaxj. 

vencl‘1  oe.a  rtnTransnatrixJ 


until  FtOutuf  droits  0«  (LeyUutLtLimiLsiax  =  VenclbdyTra  j .  eaxDdySttidx); 


IF  NOT  FtOutOf LirPitS 
THF  .< 

LegDutCfLin'itsldx  ;  =  Maxlnt 


END; 


b  EG  L  N 

FuturevehclLegsstts  :  =  venciLeusstts; 

CalcVehclLegsi ra js  ;=  True; 

FuK  Leg  ;=  Ftut  Tu  RrRt  Du 
ritGIN 

LegTrajidx  ILegJ  ;=  o; 

IF 

(Future venciLegsStts  ILeg] .  eptott  =  Support)  and 
( venciLegsCmn js  (Leg).  Sptsti  =  Support) 

Tut. < 

BEGIN 

VenclLegs lrajs  ILegJ .  RaxLegTra j lax  ;=  - 1 ; 
wIT.-i  vehclLegsTra  j  s  (Leg),  VenclLeg rra js  [Oj  DC 

b  uG  i  N 


L ft  lime  :=  o. Of 
Lftngt  :=  Q .  o ; 

Pic  lime  :=  o.O; 

fur  Vehclcraldx  ;=  Vencix  ID  VenclZ  du 
PlCPosInVehciCru  ivenclCrdloxJ  ;=  O.o; 

Cttlln>e  :=  0,0; 

CttHgt^in  :=  o.o; 

C 1 1 .1  g t a x  :=  O.o; 

FOR  tarthcralux  :=  EartnA  lu  tartnZ  uu 
NxtF nolntar tnCro  t bar tnCi dlax J  :=  u.o 

E.  :•<■(• 

C.  •'i  0 

£  bSt. 

venclLegslTa  js  [LegJ.  fiaxbeg  i  rag  Jcix  :  =  0 

END ; 

fur  Leg  :=  RrKt  Du*nTu  Ftl,t  uu 

IF  Future V  er.ciLegsStts  LLeg).  SptGtt  =  Support 

THE-) 

Ley  sOutuf  Lii'il  ts  Idxs  lueg)  ;  = 

Leguutuf  l  ircitslox  (0,  v ehcluay  i'ra  j , 

Leg,  FuturevehclLegsstts  (Leg).  Poslntartncrg ) 

EuSE 

LegsUutuf Liml tsidxs  [Leg)  :=  tfdxint; 
bdysttlax  ;=  -  l ; 

nhlLE  CalcVehclLegs'ira  js  And  (agySttldx  <  vehcidaylraj  ,  ^axadysttlax)  Du 
dtGIN 


bdysttlux  :=  bdySttidx  +  1; 

Futurelime  :=  VenclbdyTra j .  '/ehcl  BayStts  tbayot  tlax  J ,  Time; 

fub  Leo  ;=  rirRt  DU*Mu  FtLt  u u 
BcGiN 

lr  tLeqirajldx  [LeyJ  <~  VenclLegs'i  r  a  js  [Leg],  Kaxyey i ra j lax ) 

T  'i  F  "< 

It  i.F  utureVenclLegsStts  LLeg],  SptStt  =  Support; 

1  ;lF  4 
1  r 

(ruLureli.iie  >  = 

Venclueys irajs  [Leg],  vehc iLegTrajs  iLegTr a j lax  ILegJ J,  Lttiimej 

T  lit.  :i 

F  ut'irevehclbPysStts  ILegj.  SptStt  :  =  Xrtisier; 
lr  CF utare VedclLegs-Stts  lueyj.  SptStt  s  Trnsfer; 

Jih  i 
Jr 

( ?  u  t  u  r  e  J  i  m  e  >  = 

veb.cluegs  ira  js  ILegJ.  vehc.t  Ley!  ra  j  s»  lueyl ra jiax  ILegJ],  Cttiimej 

1  ;  1  Vj  i 

bcGiL 

r utur eVehclLegsotts  luegi.  SptStt  :=  Support; 
f  ut  urev  ebclLeysStts  [Leyj,  Pos inFar tnCra  :  = 

VebclLegsTrals  ILegJ.  veoclseyira js  t,cey  i  ra  j  lax  U.egjj. 

MxtF nalnLar tPCra; 

Leafrajid*  ILegJ  :=  LeyrraJIdx  luey]  i; 

Ley sUututL-iir  1  ts  loxs  lueyj  :=  Leyu ututLiF  i  ts idx  (. 
tMiycttlox,  vencirdy  L'raj  , 

lcq,  FuLurevehclLeQsstts  LLeyj  ,  fosintar there ) 

1 1 .  D 

t .  •  i  ; 


CaicVehclLeysTrajs  ;=  vehcilsStaole  (. 

VenclrioyTraj.  VehcldoyStts  (.ndysttldxj,  vehcllor.  art  h  Trans  matrix, 
Future vehclhejsstts); 

lr  ya lcvenciLeys'ira js 

l .  j  t 
br(i  J. 


Potential v ehciL egsstts  :  =  Kutureveucibegsbtts; . 
LegsCanbeoifted  ;=  True; 
b  P  l*.  A  1 

^ irLegOy t u£  clml t  s lax  ;  =  'aaxint; 

F  0 h  Leg  :=  f<fKt  DOM-llU  FuLt  CO 

IF  cegsuutOf Limitslaxs  LpegJ  <  .MinLegdutLlLim  itsldx 

1  H  F  i< 
bLGTN 

PinLegOutOtLimitslax  :  =  Leasuututuin.itsiaxs  LLey); 


LegdinLegoutCf Limits  :=  Leg 


Eat; 

IF  Ninl-eguutOfLinatsldx  <  Maxlnt 
T  n  £  >v 

BEG  IN 


Potential vehclEegsStts  [LeqJ.  SptStt  :  =  lrnster; 

IF  vehciIsStaole  ( 

Venclodylraj .  VenclBayntts  Lboysttlaxj.  VendloEartiViransfcatrix, 
Potential VenciLegsStts^ 

iHFnl 

IF  gegFootnoldl ound  ( 

PdySttldx,  VenclbdyT ra  1 , 

Leg,  PotentialVenclLegsStts  tLeqj,  PosInfcarthCro, 

VehcigegTra j ) 

THEN 

BEGIN 

VehclLeysl raj s  [Legj.  MaxLeuira j  l  dx  :  = 

VenclLegsTrajs  [Legj.  NaxLegTr a j lax  +  1? 

vehclLegsirajs  [Legj . 

VenclbeyTra  j  s  L  venclnegs'J  ra  js  lLegJ.  KaxLeg'l  ra  j  Idx )  i- 
venclueglra j ; 

LegsOutGf Limi tslaxs  ibeg)  :=  daxint 


END 
fc  LSfc 
BEGIN 

PotentialVenclLeusStts  [Legj.  SptStt  ;s  Support; 

LeqsranbeLif ted  ;=  raise 

Ei.D 

ELSE 

BEGIN 


FotentialVenclLeqsstts  ILeyJ  .  SptStt  :=  Support; 

LegsCanbeLi t ted  :=  False 
E  w  L ; 

CalcVehclLegsira js  :=  not 

C  ( b inLegHutuf  LifT  i ts lax  =  odySttlax)  AivL-  (net  LegsCanbeLif tea) ) 

E  im  D 


UoTlL  CMinLeguutOf Limitsldx  =  MaxlntJ  0*  IMJl  LegsCanbeblf ted ) 


t:W 

EnD 

E  n  i ; 


Begin 


£rd4InGperationPtr  .  SeqAddr  : =  biyc; 
brd4lnO,:eratlonPtr.'OtfAaur  :=  V; 

fcraAInOperationPtr •  AosAddr*  :=  False; 

ordilr.OperationPtr.  SegAddr  :=  far yO ; 

BroiInGperationptr.  otfAaar  :=  1; 

drasl4Conmiaieoufter6usyF romBrdyPtr •  SegAaor  :=  B1B9; 

Hr  as  1 4Con  midlebo  f  f  erBusyF  r  on>bra4Ft  r ,  OtfAaar  :=  '  t; 
bi  asi4CorTT;iale6uf  terbusyl’ rointralFtr .  SeyAaar  :=  biby; 
Brosl  4Con  T,idieBuf  f  erbusyt-  rornBraiPtr .  OtfAaar  ;=  0; 

brasl4CoiT.i>juueBufterBusyFroiiibrd4Ftr.  AosAaar*  :=  False; 

Brdsl4CoiT'i,c:tptBuf  f erPtrF  roit-Brd4Ptr .  SegAadr  ;=  fa  1  w b ; 
or  as  1 4Con  ..,c tpteuf  terPtr  Fronibra4ptr ,  uttAaar  ;=  b; 

br  as  l -iCon ’■la  leBuf  f  erPtrFrombrdAPtr,  SegAadr  i~  blnfe; 
braslACoir.r  Ioieayf  rerFcrr  roir.bra^Ptr.  CffAaar  ;=  4; 

er  os  1 4Coirr.l np tbuf  f  er Pt r f  r on  braAPvi .  SegAddr  :  =  blab; 
urcsl  4Con  i.l  nptBuf  terPtrF  r  on-BraAFtr .  Ottnaar  :=.  u; 

rrdslACo^’iCtptnutrerPtrrromordiPtr,  SeyAaar  ;=  olb7; 
or  os  l  ACom.'C  to  too  t  f  er  Ptr  rr  onnrd  1  Pur .  OtfAaar  :  =  d;  ' 

oras l -*Con  ■(  l a lebof  rer ?t r r  r on  bra lPur ,  SeyAaar  ;=  blo7; 
nr  os  1  <*Conr.,vj  aiebu  f  ter  Pt  r  r  romora  l  pv  r .  CffAaar  ;  =  4; 

erdsl*»Cun,n  I  rip t h  a  f  xer  P tr Fr  on.Hra  i  Pur ,  SeyAaar  ;=  blb7; 
oras l aCoiTii Xnptcuff er.F'trr romdraiptr#  CffAaar  :=  u; 

oras4  ICon  i..lalewut rerbusy  Fr Oititjro4Ftf#  SeaAadr  :=  du9s; 
brasAiCon  .rXalebat ter busyr  roii>bru4rtr  •  uttAaar  :=  1; 

brds4iCo(nn-iuieBuf  terbusyf  roTieraiPtr .  SeyAaar  •  -  b<>9b» 
oras4iCor.  <?■  laie  buffer  Busy  Fro  rubral  Mr.  OfiAaar  :  =  C; 

BrdsAlCon  i-..C  tptbuf  rerPtrF  roiT’ordAFt-r .  SeyAaar  :=  b09b; 
oras4  icon  i.  Ctptt'uf  f  er  PtrFrorr'Bra4ptr  •  OtfAaar  :=  d; 
brosAiCorriTialeHufferPtrt'rofftjrdapir.  SegAddr  ;=  b(’9S; 
brdsA  icon  r>  IdePuf  ter  PtrF  ron»9rd4Ptr.  otfAaar  :=  4; 

oras4lCon  r.inptHuf  f erPtrf  romord4Pur .  SeyAaar  :=  « 0 9 5 ; 
ords4lCon -inptBuf  rerPtrF  roirara4Pur,  CffAaar  ;  =  u; 

Hras4iCona.  CtptBuf  terPtrf  rombrdlPtr.  SegMoor  ;=  t>oy4; 
eros4iCon  .I tptbutterPcrr ronoraiPtr .  ottsaar  :=  d; 

H r cs4 iCon  r  l a leHuf  ter  Ptr Fr oau3ral Pur  .  SeyAaar  :=  b094; 
tras41ConiridiebufterPtrFronr.brdiPur.  OtfAaar  :  =  <*; 

oras4 1 Con  n  1  nrtt-uf  ter ?tr  r  r omaraiPur  •  segAadr  ;=  b'>94; 
o  r  a  s  a  ic  on  ri:  1  np  LHaf  t  er  Pt  r  Fr  on.or  d  1 P  t  r ,  OfiAadr  :=  u; 


c rds4 ) Con  hC" tp tK-af  t er Ptr f  r o/nardAPtr .  AosAoar*.  SeyAaar  :  =  7 7 1 1 ; 
oras4iCor  M.rtptbuf ferPtrFroir-ora«*PiLr.  AosAaor*,  CttAaar  ;=  0 

r rds4 icon it.laleeuf ter HtrF ro<t.urd4pt r •  AosAaar*,  SegAoar  ;=  7211? 

r r a s 4 1 C o n in  1  a  1  e b u f r e r p t r p r o nt o r d 4 p v r #  ADsAadr*.  CtfAdar  ;=  u 

or  as  4  icon  ri'l  np  tbu  f  f  er  Ptrt  r  on.or  a4Pt  r  .  AosAaar*.  SegAadr  ;=  6711 

nras4 lCon.iiiinptbuf f er PcrFronuird^Ptr .  AosAaar*.  CtfAdar  :  = 
hi  a  s  4  iCon -i.r;  tp  t  buf  ter  Ptr  Fr  on  oral  Pur ,  AosAaar*.  SegAadr  :  =  -bb7j; 

o  ras  4 1  Con  ...u  tptBij  f  rerPcrri  oit.sr  al  pur .  AosAaar*.  CttAaar  :=  u 

hras4iConi..loieoufterptrFrombraiPir.  AosAaar*.  SegAaar  ;=  -9173; 
a  r  as  1 1  Con  ..1  a  ieou  f  t  er  p  t  r  F  r  ondr  d  1  Pu  r .  AosAuar*.  CtfAdar  ;=  0 

or  d  s  4  icon  ;nl  np  t  ho  1 1  er  Ptr  Fr  orr.dra  1  Pur .  AosAaar*.  SegAadr  :=  -9b7i; 

bros4iCon.‘"Inptouf  ferPtrr  roir.oralPur .  AosAaar*.  CtfAdar  ;=  0 


rtras4iConi..i.jieoufrerPtrFron.ord4PLr.  AosAoai*.  AosAaar'*,  i*e*uata 


O 


Brds l  4Corrn<Ctpt ouf  t erPirFro^Bra  i  Ptr.  ADSMaar*  :  = 
drusl'iConi.ildleBufferPtrFrorrmroiPtr.  AosAdar"; 

drasl*»Coiri.:Idle3u£terPtrfron.iirQiPtr,  AosAaar*  :  = 
or  as  1 ‘♦Con  inTe'T'F.  Buf  ferPtr  ? 


dr  us  1  4Coit.ii  id  ledu  f  f  erP  ir  t  ron  br  d'tptr ,  AosAdor*.  AosAddr",  Newbata  :  = 
False; 

d r os l acoit  liluled uf  f er bus y Pr oiiidr u 4F t r .  AbsAadr*  :  =  False; 


“II m  urus 1 4CoiTir*utptbu£f erPtri1  ronibrd4PLr .  AosAddr*.  AosAddr*  lu 
Br.Gib 


1 rans « e iRqs t^ay  :  =  SqRt  (Sqr  (FrwdveiRqst )  +  Sqr  (SidevelKdst) ) ; 

It  i i ars velhqstMay  <=  VehclMaxirans Vel 

i  n  t.  v 

VelKeutcir  ;=  l.o 

t  uSi 

velr<eciFctr  ;=  Vehciwaxl rans Vei  /  Trans veiRqst nag ; 

it  turn velpqst  >  VenclMaxetot«stvei 
]  Hfc  ( 

It-  iVehcl^axPotatVei  /  rurntfeiKqstj  <  veiHeotctr 

1  .ib  < 

VelKeoF'ctr  ;=  VehclriaxRotatvei  /  Turn v elpqs t ; 

TransAccRqst^ag  :  =  SqRt  ( 

Sqr  (FrwdveiRqst  *  VelkedFctr  -  venciuinvellr venciCrd  [venclXJJ  + 
Sqr  (SIoeveiRqst  *  VelkeoFctr  -  VenciLinYelin  veticiCrd  IVencirj);  / 
CuinAlgrtriiTdxecintrvl* 


lr  1 1  ar  s AccKds thag  <  =  »encl|v’<sx'i  i  ansAcc 

Tut,, 

Accne  jt  ctr  : =  1,0 
L  u  S  t. 

AccKeoFctr  :  =  VenclAaxlransAcc  /  rransAccRqst^ag; 
HotatrtCCRqstkayjs 

Cluinveikqst  *  VelRedFctr  -  vehcl AngvellnVenclCrd  [VenclZJ)  / 
GuiaAlgrtnmbxeclntivl; 


IF  KOtatAccKqstnay  <=  vehciiiaxRotatAcc 
InL.. 

It  t  v e h c J  n a x K o t a t A c c  /  HOtatMCcRqst^ag J  <  AccReuFctr 

1  ne... 

AccweoPctr  :=  VehcldaxKotat Acc  /  RotatAcckqst^ag; 

Bros4icotT.iiiinpLBuf£erPLrFromdrd4Ptr,  AbSAddr*.  AbSAddr". 
i«e»i,'aia  ;=  False; 


IF  CalcVehClBqyTraj  ( 

1,0  *  AccReaFctr  *  VelKeaFct:  *  Fr»dvelRqst  /  GulaAigrtnmtxecIntr vl 

1,0  +  AccKedfctr  *  VeiKedr'ctr  *  SlaevelRqst  /  GulaAigrtnmbxecintrvl 

1,0  *  AcckeaKctr  *  veihecfctr  *  lurnvexuqst  /  GuiaAigrtnrr.cxecintrvl 

Currntline,  VehclTobartn Trans  Matrix, 
vehcluinvelinvehclCrd,  venclHngVellnVenciCra, 
vencl.'JdyTraj) 

Tub., 

if  CaicvenciLeqsTrajs  C 


VehclbdyTra  j  ,  VehclLegsbtts ,  v ehclijegsCtnnds , 

Vend  LeysTr  a  js ) 

Trifcw 

BKGiN 

brds4lCor>minptButterPtrFroi'iBrd4Ptr.  AbsAddr*.  AosAddr*. 
VehclbdyTraj  :=  VehclBayTraj ; 

Hrds4iCorrminptButterPtrF  ro»T>ra4Ptr.  AosAdar*.  AbsAciur*. 
v ehclLegsl ra j s  :  =  venclLeysira js; 

fcrds41Con:ir>lr.PtPuf f erPtrf‘ro"'bra4Ptr ,  AbsAadr*.  Abs*dur*. 

Newuata  :=  True 

tuD 

ELjSc 

It  Calc VenclBayTra j  ( 

0.3  *  AccnedFctr  *  VelPedFctr  *  KrwdvelHqst  /  Guid  Aigrthi’.txecintrvl 

0,3  *  AccKeoFctr  *  VelRedrctr  *  Siaevelnqst  /  GuiaAiqrttui  cxecintrvl 

0.3  *  AccKeaFctr  *  VelKeaFctr  *  Turnvelnqst  /  GuloAigr  tninExednirvi 

Currntl  1 1*  e ,  vehclXotartnlransMatrix, 

VehcluInvellnVenclCrd,  VehclMnyVellnvenclCrd, 

Vend  BoyT  raj) 

1  H  t  * 

It  Calc venclLegsTra js  ( 

venclbdy Ira j ,  VehclLegsBtts,  vehclLeysCmnas, 
vencluegs  Ira j  s ) 

TrlE.M 

BEGIN 

Brds4 iCor.iiP inptHuf ierPtrFroi»Brd4Ptr .  AbsAddr*.  AbsAdar“, 
vehcioayXraj  :=  Vehcl BayTrag ; 

BrasHlComminptButlerPtrf  ro'nbraAPtr,  AosAdur*.  AosAodr*. 

Venclueys  ira js  :=  vehclLey&Tra js; 

e r a s 4  i C o n! rr. I n p t b u f  f  e r P L r F r o I" i.' r a 4 P t r  .  AosAdar*.  AbSAddr*. 

»e* oata  :=  True 

t;,r 

E  US  L 

it-  calc vencibdyTra j  ( 

0.1  *  Accr<edFctr  *  VelPedFctr  *  F rwaveibqst  /  GuidAiqrtnn.Execintrvl 

0.1  *  AccKeoFctr  *  VeiKeoFctr  *  SlaevelHqst  /  GulcAigrtnrrExecintrvl 

C.i  *  AccKeoFctr  *  VeiKeoFctr  *  TurnvelKqst  /  GuiqAigrtnmtxeciritrvl 

C u r r r > 1 1  r rr e ,  vehdiotartnJrans.atrix, 
vehclbinVelirivenciCra,  VehciMngVeiinvenciCro, 

Vencl&ayTra j  ) 

IF  Caicvencir.eqsTrajs  l 

Vehcl poylraj,  vehcl LeysStts ,  VehclLey sCmnas  , 
vehclLeysIra js) 

In  Eu 
bt-G  i  r 

ri r d s 4 1 C o ” ft'  1  ri f  tf'u£terPtrFro"irro4ptr.  AbsAdar*.  AbSAdar*. 

Ver.clbayl  i  u  j  :=  VehclboyXrdj; 

Hr  ds 4  lCor> n> J  nptbuf  ter P ir Fro^ni  d4Ptr .  AbsAdur*.  AbSAddr*. 
vehclbeqs'I  ra  js  ;=  v  end  Leys.  1  rajs; 

Brds4 lCornninptBuf f erPtrFro‘"bra4Ptr ,  AosAdor*.  AusAaor*. 

N e * p a t a  :=  xrue 

E  i.  li ; 


IF  br ds4l  ConHTtmptButierPtrF  ru(T  bro4Ptr  .  AbSAddr 


AbsAddr*.  w  e  * !.)  a  t  a 


1  rl  E  ■ ; 

B  ilG  1  f ' 


Rl.PFA  i 

Bids4lCon>rr.IdlebufferbusyFroniBra4Ptr.  AbsAddr*  :  =  True; 

Ir  ords4 ICommlaiebuf f  erbusyFr  on-brdlPtr »  AosAdar* 

i  at  to 

Brds4lCo'T'ir'ldle6uf  t erBusyi- roniHrd4Ftr .  AbsAadr*  :=  False 
u mil  brds 4iCon>.nidleout f erctusyFrofl'dra4Ptr .  AbsAddr*; 
8ras4 lCofi'iuTenr.pBaf ier Ftr  :  = 

Dros4iCotn^inptBul£erPtrFro"|urd4Ptr.  AosAdar*; 

Bras4 icomn,inptbuf f erPtrFro»'Bra4Ptr .  AosAdar*  :  = 
Bras41CommIdleButterPtrFron'Bra«Ptr.  AosAdar*; 

r r as 4  iCoir-rnld leduf  t erPtr Fr oi"bra4Ptr .  AosAdar*  ;  = 
b  x  d  s  4 1 C  o  n.  in  T  e  i"  p  d  u  £  £  e  r  P  t  r ; 

bras4  lCOfi>niTerrpbu££erPtr  ;  = 

Bra&4lCorr(T. inptrtuf£erPtrFron'L>rdlPtr.  AosAdar*; 

bras4 1  Con  ,-,.lnp  t  Buffer  Per  Fron'crdi  Per .  AosAdar*  ;  = 
brds4lCotr,T:ioiec!o£rerPt:rFroi"braiPtr.  AosAdar*; 

or  as  4  icon  H.iaieouf  ter  Per  Fr  O"ior  u  i  p  tr .  AosAoar*  :  = 
t?  r  o  s  4  i  C  o  rr  n,  T  e  n.  p  B  u  t  £  e  r  P  e  r  ; 

brds4iCofnmIdlebu££erPerFroMibrd4Ptr ,  AosAdar*.  AosAdar*. 
t'rds4iCon.i71ialeoufferousy*Tonbrd4ptr.  AbsAdar"  :=  t-aj.se; 
erol lnOperationPtr.  AosAaoi *  ;=  True 


w  e  *  oa  t  a 


APPENDIX  G 


VEHICLE  CONTROL  COMMUNICATION  BOARD  PROGRAM  LISTING 


MUiytiL t.  iicl estate AnciCon  fi  dnnCoiT.il  unicat ion; 

PRUGRa"  V  eh  ic  leS  t  a  t  e  AndCoiTf  an  oConui.iin  ica  t  ion  llnput,  Cutput) 
C  U  S  1 

Pi  =  J.141b92t>54; 

Aealiinteger  =  a  3 ; 

ReallirtegerHusl  =  t  -j ; 

ScnnrAUtst  =  a.oc>bbo7; 

Scnnn'Cfst  =  o.o; 

ScnnrZOist  =  l.u; 

"•axod/Sttslnh  iv  1  ra  j  =  t>o; 

'  a  x  L  e ,,  1  r  a  j  s  1  n  p  e  n  J  r  a  j  s  p  e  c  s  1  u ; 

n  o  J.  r  a  j  =  - 1 ; 


UPl 

3  its  =  iPit  W,  hiti,  tit  i,  f'itj.  hit  4,  Bits,  bitb,  tit?) 
5  e  t  u  f  c  i  t  s  -  S r. T  up  t*. 1 1 s ; 

ny'te  - 
><  r  C 1 .  j  R  i  ; 

Case  integer  UK 

c;  iCnrctr;  Char); 
i;  tPitSet:  setutiitsj 

l  -I..; 

J  * C C h a r Integer  = 

K  L.  C  i  J  H  i 

C hSc  integer  up 
C ;  CLoCnar* 

h  i  C  n  a  r  :  C 1 1  a  r ) ; 

1;  Clntvai;  Inteaeri 

t  t  ; 

Crd  =  (a,  y,  L) ; 

P  t  i  r.  t  r  <J  =  A  R  P  A  Y  [  C  r  J  J  u  i  k  e  a  i  ; 
i/c  intro  =  ARRAY  iCruj  ur  «eal; 

t  a  r  t  n  C  r  o  -  Irtarthx,  tarthi,  LarthZ); 

PtinParthCru  s  ARrA*  iharthCroi  up  Real; 
v'cl  nr  or  tr  cro  =  a r  t\ a  y  Uartncro  i  nr  k e a i ; 

wehclcr  j  =  i  v ehcl a ,  VenclY,  vencl^J* 
p tlniedclcro  =  array  i  v  e  n  c  J  C  r  a  •  ur  Real; 
vclnVenclCrd  =  array  iVenclCroj  Cr'  Real; 

tbler^ri^ies  =  (id*,  Pen,  KiU; 

ur  XnK.u  1  er  ‘ingles  =  ARRAY  Iciuier  angles  J  up  Peal; 

tea.  ift  r  nn  = 

R  n  C  i  j  R  i  '■ 

Rotat:  ur x nt,u ierAngj.es ; 

Irar.s:  Rtlncra 


Trans  atrix  = 

R  t-CtJR  u 

Rotat:  ARRAY  [Crd]  UK  VclnCrn; 

Trans:  Rtincra 

K  <i  i' ; 

Trans  a  t  r l x l n  t  e  n  e  r  = 

K  iiCCRi. 

Rotat:  a R k A jf  l  C  r  d  ]  OF  array  i  c  r  d  ]  OK  integer; 

Trans:  array  [Craj  uf  Integer 

t  U ; 

Legs  =  l  K  o  n  e ,  f  1 1 1  r  F t  K t ,  CrLt.  C  r  R  t ,  k  r  L  t ,  P  r  R  t ) ; 

VencludyStt  - 
RSCuRb 

Tine:  Real; 

we n cl r c n art n Trans  matrix:  T  r  a  n  s  atrix 

KUC; 

venclndyTra jType  a 
k  r-C  i  R  4.' 

*axaaySttldx:  Integer; 

venclodyStts  :  AaRay  [u.  ,*■ axonytttsiriBdy'lra  j  J  OF  VehcledyStt 

t  \  D ; 

optsttType  a  ( T r n s t e r ,  support i; 

venclueuStt  = 
kUCuRu 

Sptatt:  sptsttlype; 

Rosin  r,  art  nCrd;  Rtinc,artnCro 

F  '.  O ; 

VenclneysSttsiype  =  array  [Leosj  OF  VenclLegStt ; 

w  enclne  }T  I  a  j  = 
h  C  C  U  H  i. 

LItTinies  Real; 
v  1 1  h  g  t :  Real; 

P  lc  j  1  .lie  :  Real; 

FiCf-osInVehclCrd:  PllnVehclCrrj; 

Cttiloe:  neal; 

Ctt-igiFlniKPal; 

CttnytRax:  heal; 
n x  1 1 n •  j  T  n V  a r  t nc r o :  P  t  J  nr  ar  t hC r a 
K  «  C ; 

Ve  nclueylrajSKec  = 
k  t  C  0  R  u 

MaxpegTrajlcix:  integer; 

v ehcl uegTra j s :  aRRA*  to. . Max neglrajs 1 nLeg f ra jsRec J  OF  VehclLegTraj 
t  .<i) ; 

Vencluegs Ira jsTyne  =  A^kAi  theusJ  OF  venciLeql ra jsRec; 

wencluegCn-no  a 
htCORo 

SptUtt :  SptSttType; 


VehcluegCn.naTraj:  Vendtegiraj 
tND; 

VehduegsCrcnd  slype  =  AR  a  a  y  [Leus  j  ok  VehciLegCirnd; 
UnitVectcrCompIntegerlniexeciAr  ray  = 

AkRAY  (-ReallintegerPiusl .  .ReaJ.llnteqerJ  UF  Integer; 

BoOieanPtr  = 

RECuh  j 

CASc.  integer  QF 

<i ;  (.ADsAdgr;  “Boolean); 

1 ;  ictf Adur  , 

SeaAddr;  integer) 

t  A  P  ; 

uni tVectorConp Integer lndexeaArrayPtr  = 
r\  L  C  D  ^  W 

C  aSr  integer  uF 

o;  (ADSHOdr:  •UnitvectorCoii'plnteqerindexeoArray) 
l ;  (CtfAdar, 

SegMdar;  T*oCher Integer ) 

EnD; 

Brasl^Corr.n Buffer  » 
rtECUKu 

ScnnrfcEartnTrans%,atrix:  TransRatrixlnteger; 
Ne*uata;  Boolean 

E  n  0 ; 

BrosiiConioBufferPtr  = 
n'lCoH  i> 

C ASc.  integer  OF 

u;  CADsAdar;  “BrdslzCo  it  in  nutter); 

1 ;  (Cf f Addr  , 

SeaAaar:  integer) 

£  n  u ; 

rirds  1  /Corrmeuf  f  erFtrPtr  = 

R  £  C  U  R  U 

CaSc  integer  uF 

0;  CAosAdar:  *Brds  l  ^Coiri’irtur  f  erPt  r ) ; 

1;  IC ft Adur, 

SeaAaar;  integer) 

t  N  D ; 

tirasl^Con  P‘>uf  ter  = 

A  EC  U A  0 

Cur  i  nil  irre  :  Real; 

V  end  lo  car  tnTrans  matrix:  Trans Fat rix; 
y enclcinvel in venclCrd , 

VenclAngvelinvenciCrd;  Vein  Vender  a; 

VenclLegsitts  ;  vendLegsStts  «  ype ; 

Fr  *dVelAqst , 

SideVelRqst , 

TurnVelKqst;  Real; 

Vend  LegsCtrnds :  VehdLegsCmn-isi  yPe*' 
k  e  *  u  a  t  a  :  boolean 
Ei<t ; 


rirdsl-iConiiHutterPtr 


NECORU 

CASr.  integer  OF 

0:  CAusAadr:  “brdsl  4Con.mdu*fer ) ; 

1:  tetfAdar, 

SeqAdar:  Integer) 

EwD; 

eras  1 lCo^mbuf terPtrPtr  = 

RECdRu  . 

C a S l  inteyer  OF 

0:  ( A  t>  s  A  d  d  r :  *Brds14CoroniburterPtr) 
1;  (.CttAdor, 

SegAdar:  integer) 

E  n  D ; 

HrosAiCorrmBuf  f  er  = 

R tCURo 

Vend  ddy'J  raj:  Vehdbdyirajiyne; 
vend Le ys T rajs:  VendoegsTraislype; 
iv equate;  boolean 
EwL; 

brasAiCoiriiiBafierPtr  = 

KbC'J’^L1 

CASfc,  integer  OF 

0;  c  AosAdar  :  *Brds41CoiTmburter); 

1:  CCtfAddr , 

SegAdar:  integer) 

■  End; 

Brds4iCo[TfiiButterPtrRtr  = 

K  E  C  J  K !.» 

CaSc  integer  OF 

0:  tAosAdur:  '*Rrds4lCorriTidurterPtr) 
1;  tCttAdar, 

SegAdar:  integer) 

t  h  C ; 

v  ak 

turrft.yChar :  cnar; 

PortCdyte:  byte; 
inptSync , 

UtptSync:  boolean; 

SyncCnar , 
inptS*ncCnarO, 

InptSyncCnar  1 , 

InptSyncCnar 2 , 

QtPtSyncCnar :  Cnar; 

U  u  it.  iii  y  k  e  <a  1 :  weal; 

EiCosProdarrayPtr, 

EiSinProaArrayPtr, 

AzCosProdArrayPtr, 

AzSinProaArrayPtr, 

ScanPtLUspArravPtr: 

linitvectcrCon.plnteuerlndexedArrdyPtr; 

drd2ln0perationptr» 

brd4In0perationPtr# 

brdiinCperationptr:  booleanPtr; 

drasWCoffihlnptBufierftrFronbrdiPtr, 

rirosliCorrrrlQleBufierPtrrron.rtraiPtr, 


»rdsl2Coir;i'0tptriuffprPtrt'rorr.BFaiPtt, 
tiros  i2Cofrrr,inptBufterPtrrrorr'Bra/»?tr, 
t3rosl2CorrmIdleBufterPLrrrorrBra/Ptr, 

Bras  1 2Corr  n,Ot  pl  buffer  Per  rrotrbra  2  Ptr :  brasl2CoirribufferPtrPtr; 

Hrdsl2CoiTmTempBufferPtr:  Brdsl/Con^bufterPtr; 

brasl2Coffri'Ialeduf£erfcusyFroniHrolPtr, 

brasl2Corr.iilaleRuffer6usyKrombro2Ptr:  oooleanPtr; 

brosi^Corr.TinptBufferPtrt'roiT.brdiptr, 

drdsl<iCotri'i:lalePaf£erPtrFro».urciJPtr# 

braslHCorr.TiOtPLHufrerPLrFroirrtraiPtr, 

nrasl‘*Con-^lnPLBuf£erPt.rf‘ronr'brd‘*Ptr, 

brasl4Con-f(,  ioleButterPtrt'rombra^Ptr, 

Rrasl4Corrr>otptHufferPiri,ro:r!bra‘*pt.r:  brasl4CorrirPufterPtrPtr; 
brdsl4CoirntlentpHuf ter Per :  brdsi<*con'Pdu£terPtr; 
brosl4Corrfi:luleBuf£er6usyProiTibr«lPcr, 
frasi‘*Con’n:laleHufferEiiJsyFrorr.Br(3  4Ptr:  oooleanPtr; 
tirds45Coirif.  inPLHuffprPtrKrorr'BrdiPtr, 
t)ras4iCorr,tldledufrerPtrf,rorrDrd'Ptr, 
bros4iCon  .vOtptbof  terPtrf  roir.or  jjptr, 
Hrds^Con^lnptbufierPtrrrorrbra^Htr, 
bras4iCorrr.'ialeBufferPtrrromBra‘*Ptr, 

Hrds4iCorr.iiDtptHufierPLrrrorr.bro‘*Ptr:  B  r  d  s  4 1  e  o  it  it  b  u  f  f  e  r  P  t  r  P  t  r ; 
cras^iConrrrienipButterPtr:  brasiJCo.’rr’dufferPtr; 
tjrds4iCorrn:laleHuf  f  erBusyFrondirdlJ  tr , 
brds4iroir.iilalebufferBusyfronr.br^4Ptr:  dooieanptr; 
Operatinq.^oae :  ct>ar; 
status:  Cnar; 

CurrntTifre:  Heal; 

yehclT  of.  artn  Trans  Matrix  :  Trans  matrix; 
daysttlox:  integer; 
bayStt IoxFouna :  Boolean; 

LastVenclPosOeqj£frurti*HiLarthC*d, 

i«extVenclPosl>eguff'ranAKftartnCrj, 

CrntvenclPosiBeoOffrarr. «Klt,artnCra;  uegutfran; 
CrntVehclvelOequf  Frort. aP  i  tartnCrd;  DeyufFraff.; 
i  n  t  r  p  1 1  i)T  i  n  e  Ha  1 1  o , 

VelCaicl jmeintrvl ;  weal; 

CrntVencl 10 harthTrans. matrix:  l r an s m a t r i x ; 
venclijinVelintartf.Crd, 

VenclAngVelint.artriCra:  vein  tar  tnCrd ; 
venclbinvelinyenclCra, 
venclAr^velinveiTCiCra;  vclni/encicrd; 
venclbinvellnt  artnCraCnna, 

VenclAngVelintarrnCroCrrna:  v c  1  n t a r t. nC r u ; 
venclbinVelinveiiClCroCiT'da, 
venclAngVellnvenclCraCn  na:  \/cln*enclCru; 

LegPoslnVenclcrd:  Ptlnv'enclCrd? 

LegsTiajlax:  ARK Ay  ILegsJ  ut  integer; 

Leoira jidxF ound:  boolean; 
scnnrPoslnVehclCrd:  PtlnVehclCrd; 

ScnnrPoslnEarthCrd:  PtlntarthCro; 

Sptottltgr:  Int.eqer; 

Legidx, 

Leginnex:  Legs; 

tar tncra  1  ridex  :  tartnCrd; 

C  ro I nae a  , 

Crdlraexl  , 

Crainaex2:  C r d ; 

VePclCralndex ;  venclCrd; 

Arrayinaexl , 

Arrayinaex2:  integer; 


Arrayinuex2XwoCnar:  T *oCnar integer ; 
VenclLegs&ptStts :  Array  ILeysj  jf  sptottiype; 

PkUCEDUhF:  InReal  C 
\/ar  ReaiVdlue:  Heal); 

r  i  p  t 

F  ourCnarkeal  = 

RtCuHj 

CASd  integer  OF 
G:  C Lowor dLoCnar , 

Lo«oraHiCner , 

Hi ^oraLoCnar , 

Hi*ordrtiCnar ;  Cnar); 

1:  (ReaiVal:  neal  j 

E  h  u ; 

VAR 

TenpRealVdlue:  FourCharKeai; 

btGIN 
KtPbA  1 

inbyt  (OCCH,  PortCByte.  Cnrctr) 

U.'iTIL  (PortCByte.  B  i  t  b  e  t  *  [  b  i  t  b  ]  )  <>  U» 
InByt  (“CsH,  Te.i.pReal  Value.  Lo*ordLoCnar ) ; 
RtPtA r 

InByt  (QCCh,  PortCByte.  Cnrctr) 

0 m X I L  (r-ortCByte,  Bitbet  *  [bit  o'))  <>  U; 
InByt  (OCwh,  i'eaipKeal  v a i ue .  Lo*ordHiCnar  ) ; 

H  c.  P  t  A  1 

Inbyt  (uCCH,  PortCByte.  Cnrctr) 
unijl  (PortCByte.  bit  bet  *  l nit 53)  <>  Lj; 
Inbyt  (uCbH,  leii.pReal  value,  rt  i  *or  dL  oCnar ) ; 

R  t  P  E  A  i 

Inbyt.  (OCCH,  PortCByte.  Chrctr) 
until  (PortCByte.  bltSet  *  Ibitb] )  <>  l J ; 
InByt  (U(oH,  TenpKeal value.  H  i  »or  dH  iCnar  ) ; 
RealValue  :  =  leiripKeal  value.  Revival 


E  f,  l!  ; 


PROCEDURE  inltqr  C 

VAh  ltgrValue:  Integer); 

’lyPE 

l*oCNarltyr  = 

RECuku 

CrtSt-  Integer  OF 
0;  (LoCnar, 

HiCnar:  Cnar); 

1 :  tltgr Val ;  Integer) 


VAH 

I  etr  p  1  tqr  v  al  ue  ;  'iwoCnaritgr; 


BEGIN 
REPEA i 

InByt  (ucCH,  PortCByte.  Cnrctr) 

U ci I i L  (Pert C Byte.  B  i  t  b  e  t  +  l ri i t a J )  <>  U 
InByt  (OCbH,  lempityrValue.  Loi’nar); 

H  EPE A  I 

Intiyt  OCCm,  PortCByte.  Cnrctr) 
u  n 1 1 L  IPcrtCnyte.  BitSet  *  [  n  i  l  5 )  J  <>  LJ 
inHyt  CoCdn,  xe^pltgrvalue.  hicnar); 

Itgr  value  ;=  'len-.pitgrvaiue.  1 1  o  r  V  a  I 
E  mj  ; 

PKUCtDUrtL  InBool  (. 

vaH  Boci Value;  Boolean); 

i  ypE 

CneCharBooleari  s 
REC JPp 

CASe.  Integer  OF 

C;  iCnarVal:  Cnarj; 

1  ;  (BoolVal ;  Boolean) 

t  u  D  ; 

VhR 

XenpBoclValue:  uneCnarooolean; 


BEGIN 


KtPbA'J 


InByt  luCCH,  PortCbyte.  Cnrctr) 
umTIL  (PortCbyte.  BitSet  *  tbitbj )  <>  U 
InByt  (uCoh,  I e;npHool Vai ue ,  CnarVal); 
BoolValue  :=  re.ppDOolvaiue.  Booival 

b  f  -  u  } 

Pf>0Cb0UKF.  inctiar  i 
vah  CnarVal ue:  Char); 

Bc-(j  I  iV 

fstPtA  r 

InByt  (OCCH,  PortCbyte.  Cnrct.  r) 

UlIU,  (PortCbyte.  bitbet  *  Loir,  jj  )  <>  (j 
InByt  ( uC BH ,  Char  value) 

E -L; 


PPOCbuUHl  Cut Peal  ( 
Healvalue;  heal); 


IXPi, 


FourCnarbeai  = 

H  c,  C  t  ’  P  u 

CASc  integer  uF 
0;  iLoworoLoCnar, 

L  o  v  o  r  a  h  i  C  n  a  r , 
ni^oraLoCnar  , 
hi^orahiCnar:  t'nar) ; 
1  :  CPeaiVal:  heal) 


VAH 

Ten-pKealValue:  KojrCharKeal; 


b  t.  G  i  h 

i'errpbealVdlue.  Kealval  :  =  weaivalue; 

K  c-  P  t.  A  i 

InByt  (OCCH,  PortCbyte.  Cnrcn) 

U.vTiL  (PortCbyte.  bit^et  *  [bito])  <>  tJ; 


Uutbyt  CUCAH,  Ter"pHeaiVdiue.  L»>nordLoC)iai  ) ; 


k  1 P  t  A  x 

InByt  COCCH,  PortCbyte.'  Cnrc>r) 

UnIIL  ( PortCbyte .  bitSet  *  tbiiui)  <>  U; 
Outbyt  (OCAH,  TempReal Value .  bo^ordHxCnar ) ; 
HLPtAT 

Inbyt  (UCCH,  PortCbyte.  ChrcLr) 
until  (PortCbyte.  BitSet  *  IbituJ)  <>  IJ; 
uutbyt  (CCAri,  Terr.pKeal  value .  H j  wordLoCnar ) ; 
HrlPc-A  1 

Inbyt  (UCCH,  PortCbyte.  Chrctr) 

UM1L  (PortCbyte.  Bitset  ♦  L  b  i  v.  u  ]  )  <>  U; 
uutcyt  (OCAti,  Temp  Real  value.  hj-voroniCnarj 

L  •*  L  ; 

PKbCtL'UKF:  Cutltgr  ( 

ItgrValue:  integer); 

1  *  P  £ 

T*oCnarltyr  = 

Fc.Ci.lkJ 

CmSc.  integer  uF 
0:  (LoCnar, 

h  l  C  r.  a  r :  C  n  a  r  ) ; 

1;  (ltQival;  Integer) 

L  -v  C  ; 

)/  A  It 

i  en »  i  igrvalue:  i *oCnar  J  tor ; 

bb(J  1 N 

Jenrf  itgrValue.  1  tgr  Val  ;  =  ltur^aiue; 

RLPBAi 

Inbyt  ( uCCH ,  ' PortCbyte .  Chrctr) 

Until  (PcrtCPyte.  bitoei  *  IBirD)  <>  lj; 
outbyt  icCAn*  Tempitgr Val ue.  LoChar)? 


ritlt-Ai 


InByt  (OCCH,  PortCt-yte.  Chrcur) 
until  (fortCtyte.  BitSet  *  CbitiO)  <>  U 
Outbyt  (OCAri,  TenpltgrValue.  iUChar) 

End; 

PrtOCt uUkE  Lutbool  ( 

BoolValue;  Boolean); 

TYPE 

CneCnar boolean  = 

KECUKo 

CASE  integer  Of- 

u;  iCnarVal:  Char); 

1;  tfaoolVal;  boolean) 

EnD; 

v'Ari 

leirpHoclvalue;  unecnarboolean; 

BEGIN 

TeiTpbooiValae.  b  o  o  1  a  1  ;=  hoolMlue; 

H  E  P  £  A  1 

InByt  (oCCH,  portCoyte.  Cnrctr) 

UNTIL  (PortC Pyte.  Bitiiet  *  tbirulj  <>  U 
uutbyt  (OCAH,  Ten-ip6ooiValue.  O'arval) 
END; 

PnCCEuUnE  CutChar  ( 

CnarValue;  Char); 

BtGI  N 
B  t  F*  t,  A  i 

Iney t  (uCCH,  portCE-yte.  Chrcrr) 

Until  (PcrtCtyte.  BitSet  *  Cultu))  <>  U 
i.jtpyt  (OCAM,  Cnar^alUP) 

E:.  v; 

Pf'UCEDUtxF  Trnsf rirPtToEartnCroFrvericU’r a  ( 

V  a  A  PtlnE  arthCrdVar ;  Ptxnr.artHCra; 
vencliOwarthlransEatrix;  xrans matrix; 


PtinvehdCrdVar:  Ptln VenciCra ) ; 


BEGIN 


PtlnEarthCrdVar  TEarthXJ 
vend  ioc.artnTrans'iatriy.  • 
ven cl ToGartnTrans Matrix, 
v  end  I  oEartnTrans  matrix . 
vend  1 ohartn Trans Matrix . 

PtlnParthCrciVar  CEarthVJ 
venclTot-artnfranSi-iatrix. 
vehclrocartn 'fra  ns  Matrix. 
vehdioc.arthTra  ns  Matrix, 
v  e  n  cl  X  o  t,  a  r  t  n  T  r  a  n  s  m  a  t  r  i  x . 

PtlnEarthCro Var  iF.artnZJ 
vehdlotartnTransMatrix, 
v  encl'icEartnTrarSxiatrix  • 
vendionartn  Trans  Matrix. 
vendiccertnTrar.S'-iaLrix. 


Rotat 

CX  J 

tXJ 

* 

Ptln  vehdCrd  ver 

L Vehclx] 

4 

Rotat 

m 

CXJ 

* 

PtlnvenciCrdvar 

C  V  e  n  cl V J 

4 

Rotat 

IZJ 

UJ 

¥ 

PtlnvenciCrdvar 

CVenclZ] 

4 

Trans 

C  x  J ; 

•  • 

Rotat 

CX  J 

CVJ 

* 

PtlnvenciCrdvar 

t vehclx] 

+ 

Rotat 

CVJ 

CV3 

¥ 

P tin venciCrovar 

C  vend  V  3 

+ 

Rotat 

LZ  J 

CV] 

* 

PtlnvenciCrdvar 

CVenclZj 

4 

Trans 

IT  J  j 

•  — 

•  ■» 

ttotat 

CXJ 

IZJ 

* 

PtlnvenciCrdvar 

ivencix] 

4 

Rotat 

IV  J 

CZJ 

* 

PtlnvenciCrdvar 

C  ver.cl  v  ] 

4 

Rotat 

tz  J 

£  Z  j 

¥ 

PtlnvenciCrdvar 

l  V  e  n cl Z  j 

+ 

Trans 

czj 

EM;; 


PKUCti'UKF,  Trnsf r ni v d o v end Cr aFr L-ar tnCra  i 
V Ah  VclnVehclCrdVar;  Vein VendCro; 

Vend  1 01-ait.nlrans  Matrix:  irans Matrix; 
VclntarthCrdVar:  vein Fart hCrd); 

BEG  1  iv 


vclnVehclCrctVar  [VenciXJ 
vehdiOc.artnTransc:atrix. 
Vend  XoEartn  Trans. Matrix, 
vend  JOL.artn  Trans  matrix. 

vclnVehclCrdVar  C  v  e  n  c  1 V  J 
v  e  h  c  1  r  o  c  a  r  t  n  T  r  a  n  s . ;  a  t  r  1  x . 
vend voEartn Trans Matrix. 
Vend  TOEartnTransMatrix . 

v  cln V  end Cr o v  ar  ivenclZJ 
v  en  dio  t^artnTrans  Matrix. 
Vend  Jot.artn Trans 'iatrix. 
vend  1  o  c.  a  r  t  n  Trans  n  a  t  r  i  x . 


Rotat 

Lx  J 

CXJ 

* 

vclntartnCrd war 

IFarthxJ  + 

Rotat 

CX  j 

l  V  J 

¥ 

VcIntartnCrdvar 

C  E  a  r  t  hi J  + 

R  o  t  a  t 

CXJ 

CZJ 

¥ 

VclntartnCrciv  ar 

IFar thZJ ; 

•  “ 

Rotat 

CVJ 

CXJ 

¥ 

VcintartnCrdvar 

CFartnXJ  + 

Rotat 

CVJ 

CVJ 

¥ 

VclnoartnCrcivar 

CfcarthxJ  + 

Rotat 

IV  j 

CZJ 

¥ 

vclncartnCraVar 

C  t  a  r  t  n  z  J ; 

• 

*  “ 

H  O  t.  a  t 

IZJ 

CXJ 

¥ 

vclntarthCrdvar 

CEartn/J  + 

ROtat 

CZJ 

m 

¥ 

VclntarthCrdVar 

CEarthVJ  + 

r  o  t  a  r. 

CZJ 

IZJ 

¥ 

vcincartnCrdvar 

CEartnZJ 

PrCCL'uUrE  CalcTrans-.atr  ixt  ron  Posnequtr'rdir.  i 
v  a  k  I  r  a  n  s  a  L  r  i  x  v  a  i  :  T  r  a  r.  s r ;  a  t  r  i  x ; 

Oea.jtt- rorr var  :  Deguf  K r  air  j ; 

V  A  K 

C  o  s  \  a  a  ,  S  i  n  i  a  w  , 

Cos Pen,  SinPch, 

CosRll.,  SinKli:  Peal; 


BEG  i  iv 


( Deyu tF run- v ar .  Kotat  [Ya*]); 
(ueyCfF ran Var.  hotat  tYa*j); 
CueyOtFranr  var .  Kotat  (Pch)); 
(oegOf F ran Var .  Kotat  [Pen]); 
(DegOfF  rdnr-Var ,  Kotat  [nil]); 
U;egCf  r  ran, var ,  Rotat  (Kllj); 


CosYa*  ;=  Cos 
sinna*  :=  sin 
CosPcn  :=  Cos 
SinPcn  :=  Sin 
CoskII  :=  Cos 
SinKll  ;*  sin 

Ira ns "atrixvar. 
Trans  matrix  ^ar. 
Trans 'atrlxVar , 

Trans'-atrixVar. 
Costa*  *  SinPch 
l ransratr ix var  . 
SinYa*  ♦  SlnPch 
Ira  ns /.atrixvar. 
(*  *)  CosPch 

Irans  ;atr ix  var  . 
CosYa*  *  SinPeh 
IransT atrixvar. 
S  i  n  Y  a  *  *  SinPeh 
Trans -atrixvar. 
I*  *)  CosPeh 

Trans  * atrixVar. 
T  i  a  n  s  a  t  r  l  x  v  a  r . 
Trans -.atrixvar. 

t  i«  l;  ; 


Potat  Ia)  l X j  :  = 
Kotat  IX)  (YJ  :  = 
Kotat  l a  j  £  Z  j  :  = 

Kotat  [Y3  £  X  j  :  = 

♦  SinKli  -  S  i  n  >  a  w 
Kotat  [YJ  IYJ  := 

*  SinKli  +  Cos » aw 
Kotat  IY)  IZ)  t- 

*  SinKli; 

Kotat  l  Z ]  IX)  :  = 

♦  CosKll  +  Simaw 
Hotat  IZJ  t  Y  j  :  = 

♦  Coshll  -  Cos V  aw 
Rotat  LZJ  IZ)  :  = 

*  Cos nil? 

Trans  LX) 

Trans  l  Y ) 

Trans  (Z) 


Cosi a*  *  CosPcn; 
Sin Ya^  *  CosPcn; 

-  s  i  n  P  c  r ; 

.*  Coshll; 

*  Cos K 11; 

*  SinKli; 

*  SinKli; 


=  uegofFrur/Var.  irans 
=  PeyUt frar»v ar-,  Irans 
=  .  utyL tt  raii-Vai  ,  Trans 


PhnCtnURF  CalcPosueyOtF  ran  F  roniTranswatr  ix  C 
V DegCf  Frair  Var  J  DeoOf  F~  ron? ; 

Trans* atrixvar:  T r a n s w a t r l x ) ; 

FiiNCTiU*  ArcTan2  l 
a  value , 

ivalue:  Real); 

Real; 


n  t-G  ]  N 


II  xvaiue  >  r».o 
ThE-. 

Arcian2  :=  Arc Tan  (Y value  /  a  Value) 

tbi’L 

ir  xvaiue  <  o,o 

The.. 

Arcian2  :  =  ArcTan  (YValue  /  AValue)  +  Pi 
ELSE 

If  YValue  >  G.O 
i  Hti 

Arclan?  :=  Pi  /  2,0 
else 

Arclan?  :=  -  Pi  /  2,0 


[>  J ; 

in; 

£2  3 


bt.Gifv 


Degftf  t  rdn  var .  Rotat  [Ya*]  :  =  Arclan2  ( 

Trans -atrix var •  Rotat  t X j  IXJ ,  Transwatr lx var .  Rotat  [) J  [YJ); 

CegOf r  roir  Var  •  Rotat  t P cr» 3  :  =  Arclan2  C 
SqRt  C 

bqr  ( i rans^atr lxVar •  Rotat  tYJ  IZJ)  +  Sqr  ( irarsRatr ixvar ,  Rotat  [ZJ  LZJ.JJ, 
-  Transnatrixvar.  Rotat  LaJ  LZj); 


Degot r  rnir  var .  Rotat  [Rll]  :  =  Arcian2  l 

Trans  iatrlxVar.  Rotat  [ZJ  [Zj  ,  Trarswatrix var .  Rotat  lY]  [ZJ); 

oegotr  rdirvar .  Trans  l x 3  :=  TransRatrixVar  .  irans  [>J; 

DegCf r  run- var .  Trans  m  :=  TransRatr ixvar .  irans  (Y); 

oeguf  fruff  Var .  Trans  i.Z)  :=  TransVdtrixVar .  Trans  LZj 

£  f;  l' ; 


b  K  G  1 :  < 

Uisafcleinterrupts; 


t?r a 2 1  nOpei  a t  ionpt.  r ,  Seanaor  :=  -  ib3Ro; 
di o2lnGperationPtr .  GtfAaur  :=  u; 

oroilnGperationptr.  SegAcar  :=  -  8  194; 
eroDnCperetionptr .  UtfAadr  :=  u;  • 

oral XnOperationptr •  SegAour  :  =  -  8194; 
rruiInCperationptr.  0 1  f  A  d  c  r  :=  1; 


crus  12Coit M.ioiebuf  terRusyt  ro;ubra2Rtr .  SeyAdar  :  =  -  lb3o7; 
«raslzCon,nTaleflufferbusyFrom6ra2Rtr.  UilAuar  :=  l; 

8rasl2Con  .niaie&uf  terbusyF  ro.i.tralrtr .  SeyAaor  J=  -  Ib3b7; 
BraslzCon-p.idlebuf  f  er&usyf romcraiRtr ,  UttAuar  :=  0; 


brasi2Co!t  ■nlaietuf  terbusyP  roiTicralktr  .  AosAoor“  :  =  False; 

crasl2Conn.Ctptbuf£erPtrFroii:brd2Rir.  SegAaar 
cr os l2Con.'r.otptbufterRtrF'ro ir.br d2Ptr.  OtfAoor 
croslZCon  n  JdleBuiterPtrFrofnbra2Ppr.  SeyAoar 
£  r  n  s  1 2  C  o  t  r  I  a  1  e  t  a  f  i  e  r  R  t  r  r  r  o  n.  b  r  a  2  P  l  r .  OffAoar 
orosTZCoiTn.lnptButterRtrrroniDraZPiir.  SegAaar 
brasl2Con'il. inptbufrerPtrrrorruraZPLr,  UfiAaar 
c  r  u  s  1 2C  on  .i,c  t  p  t  R  u  f  t  e  r  P  t  r  F  r  o  ft  o  r  a  l  p  t  r  •  SegAaar 
6rasl2CorrvCtpt6ufferPtrfro(TibrdlPrr.  ciftAaar 
crasl2Cor,y;laleBufferPtrFronr>t)rdlPtr.  SegAaar 
t'rasi2Con,„ialeHufterPtrFrofr.braipcr»  utiAaar 
brnslZCoir.TilnptnufferPtrFron.DralKcr.  SeyAaar 
prcsl2Co'Tii.lnptbufferPtrF^rorrdrdiir'ir.  CitAaar 


=  -  lc 3R a; 

=  8; 
=  -  1  b  3  b  b  ; 

=  4; 

=  -  io3«o; 

=  <*; 
=  -  1  b  3  b  ^  ; 

=  8; 
=  -  i  b  3  8  y ; 

=  4 ; 

=  -  Ib3b9; 

=  G; 


b r us  1 2Con rr.n tp t bu f fer P tr F r omor d2P r r .  ApsAodr*1,  SegAddr  ;=  81Rb; 

Brasl2Co'TiifCtptBufterPtrFronigrd2Ptr.  ADsAadr*.  0 1  f  a  a  a  r  ;=  o ; 

b  r  a  s  1 2Con  t  I  a  lenuf  f  er  Pt  r  f  r  orr  br  d2R  t  r .  ADsAaar*,  SegAdar  :=  8183; 

brosl2CorrH,ialeBufterPtrrro.vlL}ra2Pi.r.  AosAagr*.  CitAaar  :=  o; 

brasi2Co(T,i:InpLbuf terPtrFrorr«cra2Ptr .  AosMaar*,  SegAoar  ;=  blbi; 
eras  l  2Con  i,l  np  tr  uf  f  er  P  t  r  t  r  on.crdZF  l  r  ,  ADsAaor*.  CtfAaar  ;=  0; 

braslZCorr,!  t-t ptbutterPtrFron'BrdiPtr.  AosAaar*,  SegAour  ;=  -  16391; 
tr  as  1 2Co<T'nC  t  p  tBuf  ter  P  tr  F  rorrcrd  lPi.  r .  AosAaar**,  CitAaar  ;=  0; 

Bi  asi  2Coir.i,iaie6uf  terPtrF  ron’bralpir.  AosAoar*,  SegAaar  :=  -  16393; 


Brosl2Con  .nlaleEufterPtrF  rondraiPtr,  AosAaur*,  QffAdar  :=  0 

brosi2Coir.iinpt&uf  terPtrFrombrdlPer .  'AosAaar*.  SegAaar  :  =  -  16395; 

bras  1 2Con  ,i:inptBuf  ter  Ptri- ron.brui  Per  ,  AbSAcidr*.  CtfAaur  :=  0 


6rdsl2Comr.IaleBuf  ferPtrlromdraiPtr .  AosAadr*,  AcsAddr-*.  I'lefcDat.e  : 


Bras  14Con  .ijdlet  uf  f  erbusyF  ro.i.braAPtr .  SegAadr  :  =  -ol95; 
brasl4Con  i„iulebuf  ter^usyF  roiT.braAPtr  ♦  UttAaar  ;=  i; 

Br as i4Con-riialebuf  t erbusy f  ronibraiBcr .  SegAaar  ;  =  -8195; 
brosl4Cofrn,IalePaf tereusyFromoraiKLr .  OftAdar  :  =  o; 


Brasl4CoT'iTldiebofferfiusyFrom6raiPtr.  AbsAaar*  :=  False; 


Brasl4ConmCtptbufterPtrFron.tira4PLr, 
Brasl4Corri(,CtptbufferPcrPron'Dra4Ptr. 
bras  1 4Conr  n.  idlebu£terPtrrron.ord4Ptr. 
Brasl4Con.nlalerufferPtrKronu3ra4Ptr, 
Brasl4Con,n]nptBu££erPtrrronidrd4Ptr. 
brasl4Corr  r»I  nptHu££erPtrF'roni3ra4prr. 
Brasl4ConnCtptbofferPtrrro.TbralPcr, 
brosl4Co!r:rGtptBuf£erPtrrron.BralPtr. 
bros  l  4Con  ,r  ]  aipB  utter  Ptr  Fro  na-trai  Per. 
Brdsl4CormialebutterPtrFrorrbraiPtr. 
Drasl4Corr.MlnptbufterPtrFromdralPr.  r, 
brasl4Corrr.lnFtruf£erPtrFroiTbrdlPtr, 

brdsl4Cofni:CtptBufterPtrrron.bra4PLr. 
brasl4Corri~CcptBuf£erPtrFro(nbra4Per. 
Brasl4Corrn>lulebuf£erPtrFrofi:iBra4Ptr. 
brnsl4CorrrrlaleBufterPtrFrorrbra4Ptr. 
orasl4Corr,n.lnptBu££erPtrrroiTibrd4Ptr# 
6idsl4Con.i.InptBuf£erPtrFromora4kPur. 
Brasi4Conr!-.iCtpt.PurrerPtrrroftDrdlFtr. 
bras  i  4Corr  tpt  bu£  £er  PtrF  ron:rirdlPer, 
Brasl4Com'ilale8uf£erPtrFrotTidrdiPtr. 
bras  l4Con-f>:idlebuf£erPtrFroiebrai  per. 
brasi4Corr.:'inptbu££erPtrFron'draipLr. 
brcsl4Coirr;InpLBu££erPtrFro(TbrdlPcr. 

drasi4Conii.  laleBufferPtrFronidraiper. 

6rus4lCon!7aciienuf£erbusyFrombra4Ktr 
brJs4iCorT^lalePu££erbusyFro.Tfcra4btr 
Bras4iConri:.  iviiebufferbusyfro.tiBralPtr 
bias4iConr.iilJleduf£erBusyFro!7ibralHtr 

Bns4iCoiT(i.  JaleBufferBusyFrombrdiPtr 

Bras4lCortPCtptbu££erPtrFroniBrd4Pir, 
Blus41Conrr:,CtptFuf£erPtrFron.brd4Ptr. 
Brds4iConrIdleBuf£erPtrFroir.Bra4PLr» 
bras4iCoiT..  iQieBufferPtrFroiTidra4Ptr. 
bras4iConi-.Inptbu££erPtrFronidrd4Per. 
bras4 icon  iojnptBu££er Per Frofribrd4Pir. 
Bras4iCon-n.ctptbufferPtrFromdralPcr.' 
bros4lConn.CLptBu£ferPtrFron.6rdiPcr. 
bras4iCorr.wIdleBuf£erPtrfron'drdlPcr, 
bras4lCon(T.  luleBufierPtrrroii'BralPtr, 
Bros4iCorininpcpuf£erPtrFronidrdlPtr. 
Bras4iConr..lnptbu££erPtrFrombrdiPtr. 


bey Adar 

=  -  8 1 9  p  ? 

0£  t Aaar 

=  b ; 

SegAoar 

=  -619b; 

0£tAaar 

=  4; 

SegAadr 

=  -  b  1  9  o  ; 

Of f  Aaar 

=  0; 

beg Aaur 

II 

f 

cr. 

>-* 

\T 

OtlAaar 

;=  8; 

Seg aqoi 

;=  -6197; 

U££Addr 

II 

^  4 

Seg Auar 

;=  -8197? 

0  £  £  a  a  a  r 

=  U  ; 

AosAaor* 

SegAadr  := 

8147; 

Ads Adar  * 

OlfAdur  ;= 

0 

Ads  Aaar  ** , 

SegAaar  ;= 

■8107? 

Ads Aaar “ 

OtfAaar  := 

0 

AosAaor * , 

SegAoar  := 

8  0  6  7? 

AosAaar * ,  C 1  £  a  a  a  r  :  = 

0 

AosAaar*.  SeqAOGr  := 

-8237; 

ApsAaor*.  0  £  f  a  a  a  r  ;= 

0 

ads Aaar * ,  SegAaar  :  = 

-8277? 

AosAdar*.  D£fAaar  := 

u 

AosAaar*.  SeqAaor  := 

-8317; 

AosAudr * , 

GttAdar  := 

0 

AosAaor * , 

AcsAaar/‘, 

Newuaia 

segAaa  r 

;  r  -  e  3  1  b  ; 

0££ Aaar 

:  =  l ; 

SegAaar 

;  s  -  6  3  l  8  ; 

l)£  t  AuQ  r 

;  =  o ; 

Ads Aaar *  : =  False; 

SegAanr 

=  -8319; 

U£f Aaar 

=  b; 

SegAaar 

=  -0319; 

Ur  £Aaar 

=  4; 

SegAaar 

=  -8319; 

0££Aaar 

=  u ; 

SegAaar 

=  -  b  3  2  0  ; 

C£  t  Aacr 

=  8; 

SegAaar 

=  -  8  3  2  o ; 

Oft  Aaar 

=  4; 

SegAaar 

=  -  8  3  2  u ; 

0£  £Aaar 

=  0; 

False 


False 


ScnnrHosInvenclcrd  I'/ehclXj  :=  ScnnrXOist; 
ScnnrPosInVenclCru  LVehcli)  :=  ScnnrYUtst; 
ScnnrPosInVenclCra  [Vehclz)  :=  scnnrZutst; 

Ooteyt  1 0 c t h  ,  Cnr  (OHCh)); 

Cutoyt  iCCfc n,  Cnr  (005h)); 

rtiiteLn  C'lne  communication  port  is  ready.'); 

Inpt£>ync:=  False; 

CtptSync  :  =  False; 

UtptSyncCnar  :  =  Chr  lb); 

repeat 

IF  nci  lr.pt sync 

i  HhK 

bt<J  j  n 

Inbyt  (OCCH,  PortCByte.  Chrctr); 

It-  tPortCbyte.  bitSet  *  (Bits))  <>  U 

T  H  E  i. 
b  tL,  G 1 N 


lnptSyncChar2  :=  InptsyncCnar  1 ; 
Ji.PLSyncCnar  1  :=  inptSyncCnaro; 
InHyt  l uCbh ,  lnptSy ncCnaro  .> ; 


InptSync  := 

( lnptbyncCharz 

=  cnr 

12))  m  N b 

l xPpt  byncCnar 1 

=  Cnr 

Cl)) 

( inptpynccnar o 

=  Cnr 

(0)  ) 

End 

t:\L- ; 

it  nCi  utptSync 

i  n  E  '>■ 
o£G  1 W 

lnbyt  (ucch,  PortCByte.  Chrctr); 

It  (PortCByte.  bitSet  *  lbit»»j)  <>  [j 
f  h  t  ?. 

0 1.  G  i  F 

CtPtSyncChar  i-  Cnr  CJrd  (UtptSyncCnar)  -  l)? 
Outoyt  (Ocau,  OtptSyncCnarj; 

CtptSync  :  = 

(utitoyncChar  =  Cnr  10)) 


t  N  I': 


E  *l) 


UnilL  (inptsync  Ann  CtptSync); 

writeLn  (  'Communi  cat  1  ons  are  synchronized, * ) ; 


( ** 
l* 
(*1 


bet  ar  elevation  proa  net  arrays 


1 1 C  o  s  F  r  od  m  r  r  a  y  P  t  r . 
LICosProdArrayPtr. 
tiSix.ProortrrayPtr. 
tiSinProoArrayPtr, 


Sey  Add  r • 
uffAadr. 
bey Aadr . 
ufiAaor, 


IntVa  i 
LoCnar 
IntVrti 
LoCnar 


Fur  Arraylndex2  :=  0  XC  3i  DO 
ELGIN 


-  22b39  ? 
enr  toooHi; 

-  220*7; 

Cnr  Coourij; 


Arrayi.naex2'i*oCnar.  Intval  :=  «rrayindex2; 

t lCosir odnrrayPtr .  uftAaur.  Midiar  ;=  Arraylnaex2 xwoCnar .  LoCnar? 
tlSinProd ArrayPtr .  GftAaor,  nicnar  :=  Arraylr.aex2i*oCnar .  LoCnar; 

Kjh  Arraylnaexl  :  =  -Reall integer  XU  rteaiiinteger  DC 

ELGIN 


t:  ICosProdArrayPtr .  ArsAdar*  lArreylnuexi J  :=  irunc  C 

Arraylnaexl  *  Cos  (l-/b.O  +  «rrayinaex2  *  bu.o  /  31. 0 )  *  Pi  /  lAO.u)) 


hlbinproa ArrayPtr,  ADSAyor*  lArrayinaexij  :=  1  r  urc  C 

Arraylnaexl  *  Sin  ( ( -  7  b  .  0  ♦  «rrayinaex2  *  b  v ,  0  /  ji.Oj  *  pi  /  1  b  0 , 0  j ) 


L  nD 


L  ii  c  ; 


l  +  * 

(*  Set  up  azifrutn  product  arrays, 
( * ) 


AzCosProd^rrayPtr. 
A zCosPr on ArrayPtr. 
A zSinProo ArrayPtr. 
a zs in proa ArrayPtr. 


i>eyAodr  , 
Of i Aanr . 
beg Aadr . 
uffAaar, 


i  n  t  v  a  1 
LoCnar 
I  n  t  v  a  1 
LoCnar 


-  2  1  b  1  5  ? 

Cnr  C  0 0 0 H  j ; 

-  21003? 

Cnr  100'JK); 


Fl'P  Arrayiraex2  ;=  0-  TO  31  DO 
n  r,  G  I  m 


Ar  r ay  1  n je x 2  1'woCnar  .  lntvai  ;=  xrraylnaexz; 


MzCosProd ArrayPtr .  Oftador.  HiCnai  ; =  Ar r ayinaex2!*oCnar .  LoCnar? 
AzSinprooArrayPtr ,  uffAaar.  mCnar  ;=  Arrayinaex2l *oCnar,  LoCnar; 

h uk  Arraylnaexl  :=  -Rea  1 1 Integer  TO  kealllnteger  DC 
ouG  LN 

AzCuSProdArrayPtr,  AbsAjdr*  lArrayinaexij  ;=  irunc  (. 

Arrayirjexl  *  CoS  U4o.O  -  ArrayJnaex2  *  dO.O  /  3i.o)  *  Pi  /  lbO.Oj?; 
AzSinpr odar rayPtr ,  AisAaor*  iAr raylndexi J  :=  irunc  C 

Array!  raexl  *  Sin  (C<iu,0  -  Array  1  no ex2  *  bO.p  /  31, 0)  *  Pi  /  i«u.o)) 


k  u  \: ; 

l  *  * 


(♦  Set  up  scan  paint  displacement  array. 

(*) 

ScanPtDispArrayptr.  SegAdur,  Intval  :=  -  ^0493  ? 

ScanPtDispArrayptr.  OffAadr,  LoCnar  :  =  Cnr  Cuooh); 

FUR  Arrayindex2  js  l  TO  255  Du 
BEGIN 

Arrayindex2T*'oCnar .  Intval  :=  *rrayindex2; 

ScanPtDispArrayptr.  OffAadr.  HxChar  :=  Arraylndex2lwoChar.  LoChar; 

Puk  Arraylndexl  :  =  -Keel 1  Integer  TO  kealllnteyer  do 
ScanPtDispArrayptr .  ApsAddr*  l«rr aylnaexl )  :  = 

Arraylnaexl  +  Arraylnaex2  Div  Kealllnteyer 

End; 

*ritebn  ('Board  1  is  in  operation. *)  ? 

Br d l InCpera tionpt r .  Abs«odi*  :  =  raise; 

Flip  Leqldx  !=  FtLt  TO  Krkt  DO 

dr  os  1  iCon  nil  nptBuf  f  er  Ht  r  F  r  omBr  a  l  pi  r .  AbsAaar*.  Acs  Addr'* , 

V enclaegsCrrnas  ILeyidxJ.  sptStt  :=  Support; 

WHILE  True  uU 
fatGXN 

fun  Crdindexl  :=  x  10  z  Du 

FuK  Crainaex2  :=  a  TO  Z  Du 

InReal  (vehcli'OLartnlrans^atrix,  Rotat  (CrdlndexlJ  ICrd  l naex2]  ) ; 

FuR  Croinaex  : =  X  TU  Z  DU 

inReal  i  venclToc;artnTrans-iatr ix .  Trans  LCralnoex J ) ; 

FuR  vehclCralndex  :  =  Venclx  Tu  venclZ  DO 
inReal  (VenclLinVellnvenclCra  i*/enciCraindex]j; 

F uk  VehclCralndex  ;=  veneix  Tu  VehclZ  Du 
inReal  (VenclAngVelin^enclCra  rvenclCrainaexJj; 

InReai  (Cur  rnt  rm>el ; 

FuR  Crdinuexl  ;=  a  tu  Z  DO 

Fur  Crdinaex2  :=  X  TO  Z  DO 

n  r  a  s  1 2Coir  ni  inp  t  Baf  f  er  P  t  r  F  r  om6r  d  j  P  tr  .  AosAaar*.  AbsAriar*. 

Scnnr  ioEartnTrans!'iatr  ix .  Rotat  (Crdlnaexlj  LCrdindex2J  ;=  Round  ( 
venclT  Ouartnlransisatrix .  Rotat  (Crdlnaexlj  (Craindex2j  *  Real  l  integer ) 

irnsfrnPtTotartnCrdFrVenciCra  iScnnrPoslntarthCra, 
VehclToc-artnTransAatnx,  ScnnrRoslnvehciCrai; 

«rds  1 2Con  in  InrtBuf  f  erPtr  F  r  orrbra  j  Ptr .  ADSAaar*.  ADSAddr'". 
ScnnrlcuartnTransNatrix.  lrans  (XJ  ;=  kouna  ( 

Scnnrposlntartncra  (EartnXj  *  n.o j; 

i.'rdsl2Conn<InntBufferPtfFromBrdiPtr,  AbsAaor*.  AcsAdar*. 

Scniir icuartnTransiiatr ix .  Trans  CYJ  ;=  Round  ( 

ScnnrPos JntarthCrd  (Eartnfj  *  o.O); 

sras  1  ZCorrnlnptRufterPtrFro nor uiPtr,  ADSAaar*.  ApsAdar'*, 


6cnnriofc.artnlranSr:atrix.  Trans  C2J  :=  kouna  C 
bcnnr kosI ntarthCra  LLartnZJ  *  »,G); 

Kfc.PL  AT 

Brdsl^CofiimldleBufferBusyFrort.orcilPtr.  AosAadr*  :=  True; 
Ik  u-ros  12CO(rmidiebut£erdusyFron  urd2Ptr  .  AosAddr* 
erdsliCon-inialeduf  ternusyf  romcsralPtr .  AosAddr*  :  =  False 
iM-iiTL  brdsl2Cor'iTi!dleBut ferdusyr  roTHrolPtr  .  AbsAddr*? 
brdsl^CotrniTe)npr3ufferPtr  :  = 

tfrasl2Conn.lnpiBufferPtrFrotTord.iPtr.  AosAadr*; 

Bros )  2Coir .HlnptBuf  f  erPtrf  roit  ora j  Ptr .  AosAadr*  :s 
brosl^ConnilaleBufferPtrrrortrtrajPtr.  AosAaar*; 

brds  1 2Corr niidleHuf  f er Ptr t  ronr.rtrd i Ptr ,  AosAddr*  :s 
brds  1  /Con  iVrer.ipbuff  erPtr; 

Bras  1 2Cor!..Ten.pbuf f erPtr  ;  = 

drdsl/Cor  .n  InptPufferPtrr  roirdra/Ptr,  AosAaar*? 

Drosl2Con.iilnDtBufferPtrFro.TBrd/Ptr.  ADSAoar*  :  = 
ardsl/Con n-IaleHuf f erPtr F roTBra/Ptr,  AosAadr*? 

orosl/CoTi'T'jlertufferPtrFroTora/Ptr,  AosAaar*  :  = 

^rosi2 c on  TieTphuft erPtr; 

rtrdsl/Conri.IdleoufferPtrkionnrdiPtr.  AosAaar*.  AcSAdar*. 

crasl/CoT.i.ldledafrerbusyfcroii'rtrcuPtr,  AosAdar*  :  =  False; 

rtrd‘2 1  nc; deration Ptr  .  AbsAddr*  :=  True; 

TnChar  icperatingiioue ) ; 

inCnar  (SyncCnar); 

ifoyncCfaroChrCOHADi 
f  r)  t  < 

''■riteon  ('***  Conununication  synchronization  error  ***'); 
Status  :=  'a'; 
lint  Char  (status;; 

SyncChar  :  =  Cnr  (uAapj; 

uutCnar  (SyncCnar J? 

in  ( 0 e r a 1 1  n g / o a e  =  *  i  *  j 

jut  i 
Ht-Ulfi 

8rds 1 ACo^mlnrtBuf f er PtrKromri/ dl Ptr .  AosAaar*.  AosAddr* 
CurrntTiire; 

drdsl^ConinlnptRufferPtrFron.rtrdlPtr.  AosAadr*.  flDSAdar* 


Newuata  : =  True 


.  Cur rn tline  ;= 


Vehcli'oEarthTrans^atrix 


•  — 


ve n cl ToEartn Trans Matrix; 


bras)  4Coir.'ninptBuf f erPtrFrom&ralPtr,  AosAddr*.  AbsAdor*. 
Vend binvelln vencicra  :=  venciLin^ellnvenclcra; 

br as  l -tCorr minptbuf  f er Pt rErombrcilP tr .  ApsAodr*.  ApsAddr*. 
vehclAny vel lnVenciCra  :=  VenciAnaVellnVenclCra? 

Fuk  Lealnaex  :  =  FtLt  TU  RrHt  DL 
B  c.  G 1  i* 


fi.ip.  VehclCrdlndex  j=  vend*  lu  veliclZ  du 
InPeal  CLenPosIn vehclCrd  [ » ehclCrdinoexJ ) ; 

TrnsfnrPtroc,arthCrdFrVehciurd  ( 

brdsi4Copr;.lnptbuf ferPtrFroi"brdiPtr,  AosAddr*.  AosAddr*. 
Vehcluegsstts  iDeylndexJ.  PosInUar tnCrd  , 
Vencliotartrfirans/'iatrix,  uegposinv'ef.cicrd); 

lnrtcr  csptsttitgr); 

U  srcSttit-.^r  =  o 
T  1 E  u 

VericlueysdrtStts  LLeylndexj  :=  Support 

E  Li  S  D 

vencluegsSptStts  lueglndexi  :=  Trnsfer; 

br as i 4Con>iTl nptbuf terP tr Fr oii'Bra l Ptr ,  AbsAddr*.  AnsAoor*, 
Venclueysstts  tueylnaexl.  optStt  :  =  vehclLegssptStts  LLeyinaex] 

E  n  v ; 

lnH^al  tProsl  ACon'inint  tuutferPcrProincJrdlPtr .  AoSAddr*.  AosAudr*. 

{•  r  *d \i eli  Qs t ; ; 

lnneai  Cbrasl ACodn.inptbuf f er^tr FromBralPtr  .  AcsAddr*,  AosAddr*, 
Siaevelr.qst); 

Iimeai  t D an. ;ny heal ; ; 

InReal  iuurrn.ykeal ) ; 
inneai  l  C  uir.i  y*eai  )  ? 

inbeai  (pros  1  ^CofrihinptbufterPtrt  rombrdlPtr ,  AcSAdar*,  AnsAddr*. 

T a r nvelhQst j ; 

k^p  rjA  t 

t»rosli*Comf,iulenuf£erbusyFrornBrdiPtr.  MpsAcdr*  :=  irue; 

Ir  ordsl4comn.idieouf £e r Bu s v Fr omw rd 4P t r .  AesAddr^ 

Trifc.,, 

bi  ds  1  ACor.  n  Id  1  eP u f  t er nusy l  r o.nbrd  1  Ptr  .  AosAadr*  ;  =  False 
U.iTlL  ErdsMCon.iiJulebutferousyFrorobralPtr.  AbsAadr"; 
brdbl  4  C  o  it,  hi  ’i  e  p  b  u  f  r  e  r  P  t  r  ;  = 

brosl^Corr'riinptbuf.terPtrFromdraiPtr.  AoSAddr*; 

Bros  1 4Con\r. lnptBufierPtrrroi'uiralPtr.  AosAodr*  :  = 
rirdsliCo'r.r.ialebufferPtrFrorrtiralPtr.  AusAaar*; 


erasi^Coirn.idledufferPtrFroivrirolPtr 
pros  l  ‘iCor.  niTef'pbutferPtr ; 


Aos Aaar 


nrds  1  'tCorcinTer-p&uf f erPtr  :  = 

Bras i  ^Com.nlnptBuf  rerPtr  f  romBra4Ptr  •  ApsAddr*; 

Brasl^ComrninPtriuf ferPtrFrofr>ord4Ptr,  AosAadr*  :  = 

Brds  1  tCon'. r,i jieBufterPtrF romrird4Ptr ,  AosAadr*; 

eras  1  ACorrr  IaleBuf  f  erPtrFron'draAPtr ,  AosAaar*  is 
tiros  l  -iCuni.nTei'Vouf  ierPtr ; 

eras J 'iCon.rr.j dl eBuf f erPtrFroniurd l Ptr ,  AbsAddr*.  AosAdar*.  Ne*uaid  :  = 
Krosl  ^Con  'iiIuleBof  terbusyF  ro-fbrdiPtr.  AcsAddr*  J=  False; 
Brc^lnCperationPtr.  AosAdar"  ;=  True; 
oral  lr.Cperationptr .  Aosaaar*  ;=  False; 

*  r  1 1  e  L  r  ('Hoara  4  is  in  operation. 

NlP  i.  A  1 

U:j]lLBrdlInOperationPtr.  AosAaar*; 

eras 4  l Connl  ul eouF  ter t* usyF  r or<->r a  1  Ptr .  AosAddr*  :=  True; 


Ht-PcA  j' 

U  -HL  NJT  brds4iCominldlebuf  ferausyfro..  brd4Ptr .  AcsAddr*; 


it  urds  ilCoffir  idiebutf  er^trFroir.brdiptr .  AosAdar*.  AbsAddr*.  we*oata 
I  fit  . 

Be  Gil 


Comi'i e^pBufferptr  ;  = 

Frds41CornOtPtBufterFtrFro'',BrdlPtr.  AosAdar*; 

c rcs4 iConr.^f' tpt»uf t er Ptr Fro'"brd  1  Ptr .  AusAdor*  ;  = 
Bras4iCor,r, iaiebutterPcrFron'oraiPtr.  AosAdar*; 


cras4iCoiT.,iJalerufterPtrFro"iDr&lPtr.  AosAcar*  :  = 
bi  a  &  4  iCoir.v,  iebpbutierPtr ; 

FrasAlConmTen-phufterPtr  i  = 

erds^iConn'r.'tPtb ufferPcrFro"*Brd4Ptr.  AosAdar*; 


BrisA iCoa„rtMt,BtitierPrrFro",Dro4Ptr.  AosAdar*  :  = 
orasAiCorn  JaJeoutterPtrFro'i'oraAptr .  AosAaar*; 


Bras4iCon;f, ialeBufferPtrFro'"bru4PLr.  AosAdcr*  ss 
erosAlCorririTerpnuttert’tr; 

t>rd&AlCoPiTlale»uf fer?trFro"!brdlPtr .  AosAdar*.  AosAadr*.  r.  e  *  u  a  t  a 
False 


t  Hi; 

uras4iCort,.ifjxeoufferBusy!-roM!PralPtr.  AosAadr*  :  =  fraise; 

B'lyottiaxt  ojn d  :  =  False; 
o  i  y  o  t  c  l  u  x  •  =  <  i ; 


true  ; 


witli.f  ("dl  l  odySttldxFound )  A"<b 
(  r*  d  *  S 1 1  jl  a  X  < 


AbSAadr*.  AbSAddr*.  Ver.clddyTr a j 


dras-HCo'NiiOtptBuf  ierPtrfromoralPtr . 

Max ddyS L t lax )  bO 
1  F 

(Currntiifne  >  = 

Brds4iCon.T1iitPtbufferPtrFron'ttralPtr,  AtsAadr*.  AcsAanr*.  vencludyTra  j , 
VendndyStts  [Bayottldx  +  lj.  Tl/re; 

f  H  r 

Baysttlux  :=  bdySttldx  +  1 
ELSL 

BaysttlixFouna  :  =  True; 

Cal cPosuejuf FrctRiFronilrans matrix  ( 

LastVenclPosuegUflrdm»fiXEartncrd# 

ards4lCoiTiii>utPt6uf  lerPtrKro/r.yr  alPtr,  AbSAddr*.  AosAdnr*.  Vehclnoyl raj . 
vencibdyStts  IFdySttloxJ*  v end  lobar tn lrans«a tr ix  ) ; 

IF 

BdySttldx  = 

Brds4iCon mOt-ptBuf  ferPtrf  ron brciiPtr  *  AosAadr*.  AosAddr*,  Vehclbdylra j , 
naxudySttldx 
1  n  1 1') 

BcJGiF 


CrntVenclPosdeoOfFrcfm.vRfe.aithCrd  :  =  LastvenciPcsUequfFrdfp*RltartnCrd; 


CrntvehclVelDegdtr rom^H ftarthCra.  Rotat  lla*J  :=  u.C; 
Crnt vend veiDegOf F rcjmwKTtartnCra.  koui  (Peru  ;=  u.u; 
Ci  ntvencl  VelDegJf  r  ran-*R'itarthCra,  Rot-dt  [RllJ  :  =  u.u; 
CrntVencl veiuegufF ramARltortnCra,  Trans  [xJ  :  =  u.u; 
Crnt Vend velDedu'f Fran./RTtartnCr j.  Xrans  UJ  :  =  0,0;- 
CrntVencl VeiDeQSf r ycaitr.Cra,  Trans  [21  ;  =  o.o 


E  i.  f« 

E  uo  r 
dt.GlN 


Ca  1  c  F  o  s  ue  q  U  t.  F  r  d  rn  F  r  o  a>  !  r  a  n  s  a  a  t  r  1  x  C 
uextvenclFos))edOfFrdn»vKTt,aithCrdf 

dra&4iconii,otptt>uf terPtrFron'brdlPtr.  AtisAddr*.  AbSAdar*.  VeRclbayTra j . 
vehclBdyStts  LBoyattlax  +  ij,  vehdloEartn'lransMatrix j ; 


IntrpltmimePatlo  :  = 

(Currntil v e  • 

Br  as4  IComri.Gtst  Bui  t  er  PtrFr  oii’tirdl  Ptr .  AbsAdar*.  AosAadr*.  VenclnayTra  j. 
venclodystts  [BdysttldxJ.  Mne)  / 

(Dras41Coirr'0tptButterPtrFr«ff;hrdiPtr.  AbsAaar'*,  AbsAucir*.  venclboyTraj 
VenclBdyStts  [BuySttldx  +  jJ,  *j  1  rr e  - 

Bros4  lCofrjrC'tr  tbuf  f erPtrFrof'tirai ptr ,  AbsAdar*.  AbSAddr*.  VehclBayTraj, 
Ver-cluoyStts  IBaysttluxl.Jirre); 


Cr  ntv  enclPosPequfFrdn.^RTLarthCrd 
Las t Vend Posi-egofr  rof'-^Rl  tar tnCrd 
n  e  x  t  v  e  n  cl  po  s  OegG  f  r  r  on* «  n  Vta  r  t  nC  r  a 
LastvenclPosi'ecjufFraff  «RrtdrtnCra 
CrrtvenclPosf'eQutFraiT.  *FTe.artnCrd 
LasivehclPosPeQ<jfFran.*kTEartnCrd 
uextvendPosUeydfFron.  *«Tc.artnCra 
LSst.vendRosL'e9ufrro(r"'RTc.artnCra 
CrntVencl  PosnegufFr-dniwftTEartnCra 
LastvenclPosuegufFrun*RrtaitnCrd 
wextVenclPosUegJfFramrtRTbartnCrd 


KOtdt 

[  i  a  *  J  •  - 

KOtat 

trad  ♦  ( 

Kotat 

[  Y  3  *  j  • 

KOtdt 

[  idw]  )  * 

intrpitnTlmeRatio 

Kotat 

I Pch J  :  = 

Rotat 

[  F  c  r,  j  +  ( 

Rotat 

[Pen]  - 

Rotat 

[Pen j  )  * 

Intrpltn'iimekatio 

Kotat 

[will  ;  = 

Kotat 

[Kill  +  C 

Rotat 

1*111  - 

LastvehclPosOequf  frqni«*RTEarthCra,  «oiai  [«liJ  j  *  Intrpltnii.reKatio; 

Cr ntvenclPosoegutF  ron:*RTEarLnCra,  Trans  (aj  :  = 

LastVenclPosPeg J£F r  jn  *r'Te.ai  tnCro ,  lrans  t a J  +  ( 

Next venclPosDeaUf fron  "RTc.arthCra.  Trans  [A3  - 

Last VehclPoskegUf Frdn ^RTEarthCrd .  Trans  Ca]j  ♦  intrpltnTinieRatio; 
CrntVehclPosOegjf  j- rd^i-vRIhartPCra,  Trans  U3  *  = 
LastVenclPosuegdifFron^RTc-artnCra,  T'rans  C  x  3  +  ( 
NextvehdPosPeguffranfwRTEaxtnCra,  Trans  t x 3  • 

LastVenclPosOegu£Pram*RTtai  thCra .  Trans  [Y3j  *  lntrpltm  in.ekatio; 
CrntVenclPosL'egOfFrtjm-'fkTLarthCra.  Trans  (Z3  :  = 

LastVehdPosDegGf  Fran  wRTL'artnCra ,  Trans  [2]  +  ( 

NextVendPosleguf  Fran,*RTt,artnCra.  lrans  t Z 3  * 

LastVehclPosueg«-)£FrGn.AkTtarcnCrc!.  Trans  iZi)  *  lntrpitniimeKatio; 
VelCaicT'imeintrvl  :  = 

Hrds4 ICoM'OtctPuf  f erPtrFroi'ibrdl Ptr .  AnsAddr*.  AosAadr*.  vehcltidyTra j  , 

Vehcl'JdyStts  IPJydttidx  +  ij,  Tine  - 

CurrntTinre; 

CrntVencl  veii)eau£F  run  *r  TtarthCrd.  Rotat  tfawj  :=  c 
Next  venclPosDeguf  r  rniTi^HTtartnCrd,.  Kotat  t  V  a  *  3  - 

CrntvenclPosueguf  F  ran'*KTe,arthCrd.  notat  liawj)  /  velCalcTirneJntrvl; 
Crnc vend veiDegOfF rgn^RTtarthCrU.  Rotat  [Pcn3  :=  ( 
uex t venclPosoegjf Fr an *RTEartnCrd.  «ot<it  [Pcn3  - 

CrntVenclPosDedOf F ran *RTEaitnCra,  «otat  [rcni)  /  VelCald imeintrvi; 
Crnt  v  encT  veiuegofFron  .vr  rtaitnCro.  Rotat  [Rlli  :  =  ( 

NexdendPosuegJf  r  ron,«R  TEarthCra,  Rotat  C k  1 1 3  - 

CrntvenclPosT'eqOfr  run.^RTtarthCra,  Rotat  (Rlli)  /  VeiCaicTimeJ ntrvl; 
CrrtVer.clvelDeguf  FrdiT.vRTEartnCrd.  lrans  [a]  ;=  ( 
iVextvenclPosDegCf  F  rurr>«pTEartnCra,  Trans  C a 3  • 

CrntVenclPosDegOfr  ro<T.AKTtdrtnCra,  Trans  [Aj)  /  veiCalcTi  n.el  ntrvl; 
CrntVendveiOeguf  r  ron«RTt.artnCr  a,  Trans  [YJ  :  =  C 
N ex t Vehcl PosbegDf  t- rna' a hTtar t  nCr a ,  Trans  t  x  3  - 

Cr  ntvehdPosi'egiJf  fr  ran.*»pTtar  thCrd.  lrans  C  x  3  3  /  veiCalcTinelntr  v  1 ; 
CrncVencl VeiuegdfF raii'^RTtartnCra.  (rans  [23  ;=  ( 
ivextVendPosnego£r  rorriwRTcarcnCra,  lrans  [Z3  - 

Cr n t vehclpos uequf FrniT. a RTtar tnCr a ,  Trans  tZ3)  /  VeiCalcTimeintrvi 

F  *»  li  f 

Calc!  rans  -iatr  ixt-  ro'rPosi'egOtFrun  ( 

Crnt Vend  toLar tn  1  ransna trl x , 

CrntVer  clPosl'egiifFram^RIFartnCro); 

vencluinvelinc-ar  tnCraon  nd  iF«rtnxj  :  = 

Crnc  Vend  veiL'equf  F  rdm..Rrtar  tnCro.  lrans  1X3; 

Vetcliil  n Vellnc-artnCr dCn.no  IFdrtnYJ  :  = 

Crnt venclVeiPenufFran-vRitartn era.  rrans  ill; 
vend  i.ln  Vel  Tnc.ar tnCroCirnd  Lf-artnZj  ;  = 

Crntvencl veiDeQjfr'r-q.T. ARTFartnCro,  Trans  c z 3  ; 

lrnsfrtrvcTovendCrdFrcartnCro  ( VencloinvellnVendCruCmna , 
CrntVenclTOtartnTransi,’atrix,  v  ehdLinveim  Partner  a  C  rr.  n  d ) ; 

VehcJAngVelinVendCrdCund  ivendXj  :  = 

Crnt  vend  veiDegof  Fran  rtartncra.  wotat  [Rlii; 

VehclAngvelm venciCrdCn.no  iVenclYJ  :  = 

Crnt  vend  veii)equ£Frarr..'.rl  Ear  tnCrd .  Kotat  iPcnJ; 

VehdApqVelinvenciCroCii  rid  LVehdZ3  :  = 

Cr  n  t  vend  ve  l  DegnfF  ran -nr  i  Ear  tnCra .  Rotat  iYa*J; 


FUR  Legldx 
oc-GlN 


FtLt  10  KrRt  DU 


Legxra j idxFouna  :=  talse; 

LegsTrajIaxlLegI<axJ;=0; 

WnlLF  (NOT  Leg! ra J 1 dxFo una )  AND 
( begs  I  ra j idx  (Legloxj  < 

brds4iComiii0tntBuf f erFtrFromBrdlPtr.  AosAddr*.  AbSAddr*. 
Vencluegs irais  (LeqldxJ,  .vaxLegTr a j lax )  DO 
IF 

( Cur r ntT line  >  = 

Bras41Com1riutptAuf  lerRtrF  ron'BrdlFtr.  AbsAdar*.  AosAddr*. 
vehclLeysl rajs  (Legiax],  VehclLey'ira  js  (Legs')  raj  lax  (LeglaxJJ. 

T rtf  n 

Leas'j'ra jldx  iLegldxj  :=  Leoslrajiax  ILeglaxj  +  1 
EuSu 

Leg Tra j IdxFound  :=  irje; 

IF  L  e  y  s  T  r  a  j  1  d  x  l  L  e  a  i  a  x  J  > 

brds41Com,!,otptBuf  f erPtrFroi»Braj  Ptr .  AasAddr*.  AosAadr*. 
VehclLegsira 1 s  (Leglaxj,  KaxLeaTra j lax 

IriFn 

Leaslrajldx  (qeylaxi  :=  Noiraj 
E  L  S  b. 

li  NUT 

(CCurrntTiwe  >= 

Br ds 4 lCo'nc.otptBuf  ter PtrFron'Brd l  Ptr ,  AbsAddr*.  AosAdor*. 
VehclLegsira js  (Legldx],  VenclLeyTraj s  ILegsTrajldx  iLealdxJJ, 
Lttiin.e] 

AND 

(CurrntTin.e  < 

Rras4 lCorr^.OtPtBuf terPtrF ro»'bralPtr ,  ADsAdar*.  AbSAddr*. 
Vehclueysirajs  iLegiax]  ,  V wnclLegTra js  LLegsIrajlqx  (LeglaxJ], 
cttriu.e)) 

T  h  E  N 

LegsTrajldx  (LeqlaxJ  :=  Noj'raj 

EnD  ; 

FUR  Crdindex 1  :  =  X  1C  Z  DO 
F  u  R  Crdlnaex2  :=  X  1  u  Z  Du 
*llu  Crnt venclToEartni rans^dtrix  uu 
Gutneal  (Rotat  (CrdlnoexlJ  (Crainaex2Jj; 

FOR  Crdindex  :=  X  lu  z  uu 
w  1 T n  Cr  n t  vehc lToEar  th  Ir ansMai. r  1  x  DO 
Qutneal  (Trans  (CrainaexJj; 

FOR  VehclCra Index  :=  venclX  x u  VehclZ  DU 

OutKeal  (  venclLin  vellnvenclCi  uCdina  L  venclCra  I  ndex  J  ] ; 

FuR  VehclCrdindex  :=  venclX  jG  VehclZ  DU 

Gutneal  ( VehciAnqVellnVenclCrdCnmd  ( VenclCrd Index j  ) ; 

f uk  Leqidx  :=  FtLt  TG  Rrnt  DO 
ri  c.G  1 N 

IF 

(Legs Irajldx  (LegidxJ  =  NoirajJ  UR 
( venclLegsSptstts  (Leglaxj  =  TrnsterJ 


CttTine) 


ImE*  . 
fa  LG  IN 

Outltyr  (urd  (Nu'i  True))? 

BrdsHCorm.inPtDUf  terPtrt'ron  drdlfatr.  AosAdar*.  AosAdar*. 
VehclLegsCmnds  LLeglaxJ,  faptstt  :=  Support 

LrfL! 

tLSp 

BoGiN 


uutJtgr  (urd  (not  False)?? 

PrdslaCo^.T,  J  notfauf  ierPtrt’ ron  ordlFtr  .  AdsAadr*.  AusAdur*. 
vehclceqsCninds  LLegidx)  .  SptStt  :=  Trnsfer 

L„i,; 

lc  uCT  ( Legs'! ra j lux  ILegldxj  =  KoTraj) 

'IfiE'j 

far ds  1 4C oil’ll: InP t but t er Ptrfr  romar a  1  Ptr ,  AbsAdar*.  AoSAddr*, 
v enclLeysOnds  ILeqlax).  VenclLegCn.nd ire j  :  = 
far  as4  lCoinn  otptfau  f  ter  Ptr  Fro'i'ora  l  Pt  r  .  AosAdar*.  AosAciar*. 
vencioegs fra js  LLegidx).  VenclLeg  lr  a  js  ILegsIrajlux  (Legldxj); 

*>11  .i 

faros  4  iCor"fi  -OtPt  But  ter  Ptr  Froward  1  Ptr .  AusAdor*.  A&sAddr*, 

V ehcloegs l ra j s  ILeqldxJ.  VenclLegTra js  ILegsTrajlox  (LegldxJJ  DU 
BOGiN 

OutKeal  (CttTirre); 

(*  OutKeal  ( Lf  tTin.e )  ? 

* )  OutKeal  (LftHgt); 

( *  OutKeal  (PlcTin'e)? 

*)  F uk  venciCralnoex  :  =  tfencix  to  */enclZ  DC 

OutKeal  (PicPosinVenclCru  LVenclCraindexJ); 

1*  OutKeal  (Cttlime); 
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APPENDIX  H 


FORCES  ON  SPOOL 
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FORCES  ON  SPOOL 


Moving  Forces 

Spring  Force 

F  start  =  <Ax  =  88  Ib/in  (0.258  in)  =  22  lbf 

F  finish  =  <Ax  =  88  lb/in  (0.883  in)  =  78  lbf 

Rod  Area  Differential  .2  ,n  .  \2 

•rr d  tt( 0 . 25  in)  0 

F  =  PA  A  =  “4“  =  4  =  0.05  in^ 

F  max  =  4000  psi  (0.05  in2)  =  220  lbf 
F  min  =  100  psi  (0.05  in2)  =  6  lbf 

Opposing  Forces 

Seal  Force  (from  two  components) 

F  seal  =  F  compression  +  F  pressure  =  moving  force  on  seal 

F  compression  =  1.5  Ib/in  (0.25  in  )  n  =  1.2  lbf 

F  pressure  (4500)  =  70  lb/in  (3000  P|j)(0.375  in ) 2( . 785)  =  11.6  lbf 

(100)  =  0.25  lbf 

Static  Friction  is  approximately  3  times  moving  friction 
F  seal  moving  =  1.5  lbf  0  100  psi 

=  12.8  lbf  @  4500  psi 

F  seal  static  =  4.5  lbf  @  100  psi 

=  38.4  lbf  (?  4500  psi 

Hydraulic  Drag  Forces 

F  hyd.  Drag  =  APA  A  =  (1.0  in)2  (.785)  -  .785  in2 
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A  P  is  a  function  of  flow  and  hole  size.  For  the  spools  there 

\ 

are  six  11/64-inch  holes  used  for  moving  fluid.  The  flow  is  the  volume 
of  fluid  moved  in  the  valve  shift  time. 

Q  =  (0.785  in2) (0.625  in)/0.01  sec  =  49  in3/sec 

=  12.7  GPM 

We  can  find  the  pressure  drop  by  looking  at  flow  through  ori¬ 
fice  charts 

A  P  =  13  psi 

F  hyd.  Drag  =  (13  psi ) (0.785)  =  10.2  Ibf 

2 

Since  this  force  is  proportional  to  Q  then  this  would  be 
the  worst  case  because  we  assumed  a  10  msec  shift  time  instead  of  50 
msec,  the  actual  force  would  be  much  lower. 


March  13,  1984 


Batteile 


Columbus  Laboratories 


505  King  Avenue 
Columbus,  Ohio  43201 
Telephone  (614)  424-6424 
Telex  24-5454 


U.S.  Army  Tank-Automotive  Command 
Attention:  DRSTA-ZSS 
Warren,  Michigan  48090 

Dear  Sir/Madam: 


Reference:  Contract  Number  DAAE07-83-C-R040 
DARPA  order  number  4670 

According  to  the  contractual  requirements  copies  of  the  Final  Report 
on  the  referenced  contract  were  distributed  as  indicated  on  the  attached 
distribution  list.  The  corrections  were  made  according  to  telephone 
directions  from  TAC0M. 

Batteile  Columbus  Laboratories  has  enjoyed  working  on  this  program  and 
look  forward  to  participating  on  future  programs.  Project  questions  should 
be  directed  to  Dr.  Richard  K.  Thatcher,  who  can  be  contacted  at  the  above 
address,  or  by  telephone  at  (614)  424-7750. 


Sincerely, 

Richard  K.  Thatcher 

Associate  Section  Manager 

Digital  Systems  and  Technology  Section 
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cc:  U.S.  Army  Tank-Automotive  Command 
Attention:  DRSTA-I,  Buyer  Code:RRRD 
P  &  P  Directorate 
Warren,  Michigan  48090 

Defense  Logistics  Agency 
DCASMA,  Dayton 

Attention:  Mr.  Jesse  L.  Richey,  DCR0-6DCA-J3 
c/o  Defense  Electronics  Supply  Center 
Dayton,  Ohio  45444 
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Defense  Documentation  Center 
Cameron  Station 

Alexandria,  Virginia  22314  11/1 
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